#if defined(TSINGMA_MX) || defined(TSINGMA_GX) || defined(ARCTIC)
/**
 @file sys_tsingma_mx_qos.c

 @date 2018-9-21

 @version v1.0

 The file contains TMM QoS APIs of sys layer
*/

/***************************************************************
 *
 * Header Files
 *
 ***************************************************************/
#include "sal.h"
#include "ctc_error.h"
#include "ctc_debug.h"
#include "sys_usw_common.h"
#include "sys_usw_chip.h"
#include "sys_usw_qos.h"
#include "sys_usw_dma.h"
#include "sys_usw_stats_api.h"
#include "sys_usw_port_api.h"
#include "sys_usw_port.h"
#include "drv_api.h"

#define SYS_CHECK_LVL_GROUP_NODE(group_node, level) \
    {\
        if (CTC_QOS_SCHED_GROUP_LVL_Q == level) \
        { \
            CTC_MAX_VALUE_CHECK(group_node, MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NODE_NUM) - 1); \
        } \
        else \
        { \
            CTC_MAX_VALUE_CHECK(group_node, MCHIP_CAP(SYS_CAP_QOS_LX_GRP_NODE_NUM) - 1); \
        } \
    }
#define SYS_GET_QUEUE_BASE_BY_GROUP_ID(group_id, queue_base, queue_num) \
    {\
        if (group_id < MCHIP_CAP(SYS_CAP_QOS_QUEUE_GRP_BASE_16Q)) \
        { \
            queue_base = group_id * MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP); \
            queue_num = MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP); \
        } \
        else \
        { \
            queue_base = MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_16Q) + (group_id - MCHIP_CAP(SYS_CAP_QOS_QUEUE_GRP_BASE_16Q)) * MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP_16Q); \
            queue_num = MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP_16Q); \
        } \
    }

#define SYS_QOS_SCH_LVL_GRP_NUM(level, num) \
    { \
        uint32 _lvl_grp_num[CTC_QOS_SCHED_GROUP_LVL_MAX] = \
                           {MCHIP_CAP(SYS_CAP_QOS_L0_GRP_NUM), MCHIP_CAP(SYS_CAP_QOS_L1_GRP_NUM), \
                            MCHIP_CAP(SYS_CAP_QOS_L2_GRP_NUM), MCHIP_CAP(SYS_CAP_QOS_L3_GRP_NUM), \
                            MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NUM)}; \
        (num) = _lvl_grp_num[(level)]; \
    }
#define SYS_QOS_SCH_FLAG_FLUSH_EN  0
#define SYS_QOS_SCH_FLAG_WRR_EN    1
#define SYS_QOS_SCH_FLAG_SP_DIS    2

#define SYS_QOS_SCH_ADJUST_STATE_CHECK_TIME1   50
#define SYS_QOS_SCH_ADJUST_STATE_CHECK_TIME2   500
#define SYS_QOS_SCH_CLAER_DEFICIT_TIMES        20

extern sys_queue_master_t* p_usw_queue_master[CTC_MAX_LOCAL_CHIP_NUM_PP];

extern int32
_sys_usw_qos_get_policer_rate_and_threshold(uint8 lchip, uint8 cos_index ,ctc_qos_policer_t* p_policer_param, sys_qos_policer_t* p_sys_policer);

extern int32
_sys_usw_queue_alloc_ext_group(uint8 lchip, uint32 num, uint8 multi, uint32* group_id);

extern int32
_sys_usw_queue_alloc_ext_group_from_position(uint8 lchip, uint32 num, uint32 group_id);

extern int32
_sys_usw_queue_free_ext_group(uint8 lchip, uint32 num, uint32 group_id);

extern int32
_sys_usw_queue_drop_write_profile_to_asic(uint8 lchip, uint8 wred_mode,uint8 is_dynamic,
                                             sys_queue_node_t* p_sys_queue);
extern int32
_sys_usw_queue_drop_read_profile_from_asic(uint8 lchip, uint8 wred_mode,
                                             sys_queue_node_t* p_sys_queue,
                                             sys_queue_drop_profile_t* p_sys_profile);
extern int32
_sys_usw_check_queue_status(uint8 lchip, uint16 que_id);

extern int32
_sys_usw_queue_get_ext_queue_enable(uint8 lchip, uint32 gport, uint8* stacking_queue_en,
                                                     uint8* bpe_en, uint32* dot1ae_en, uint8* ext_que_en);

int32
_sys_tmm_sch_set_group_config(uint8 lchip, uint32 group_id, uint8 flag,uint8 skip_level, uint8 enable);

int32
_sys_tmm_sch_unbind_sched_group(uint8 lchip, uint32 child_id, uint32 parent_id);

int32
_sys_tmm_sch_check_chan_bind_root_group(uint8 lchip, uint8 chan_id, uint8* enable, uint32* group_id);
int32
_sys_tmm_sch_check_chan_bind_queue_group(uint8 lchip, uint8 chan_id, uint8* enable, uint32* group_id);

int32
_sys_tmm_sch_create_sched_group(uint8 lchip, uint32 group_id, uint32 parent_id, uint8 chan_id, uint8 is_init);

int32
sys_tmm_sch_create_sched_group(uint8 lchip, void* p_sched_grp, uint32 chan);

int32
sys_tmm_sch_destroy_sched_group(uint8 lchip, void* p_sched_grp);
int32
_sys_tmm_sch_clear_deficit(uint8 lchip, uint8 level, uint32 sub_grp_id);

#define __POLICER__

STATIC int32
_sys_tmm_qos_policer_get_opf_index(uint8 lchip, sys_qos_policer_t* p_sys_policer, uint8* index)
{
    CTC_PTR_VALID_CHECK(index);

    if (DRV_FROM_AT(lchip) && p_sys_policer->glb_policer)
    {
        /*global policer, opf index 6-9*/
        *index = p_sys_policer->policer_lvl + 6;
    }
    else if(p_sys_policer->sys_policer_type == SYS_QOS_POLICER_TYPE_GROUP_FLOW)
    {
        /*group policer, opf index 4-5*/
        *index = p_sys_policer->policer_lvl == SYS_QOS_POLICER_IPE_POLICING_0 ? 4 : 5;
    }
    else
    {
        /*normal policer, opf index 0-3*/
        *index = p_sys_policer->policer_lvl;
    }

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_free_statsptr(uint8 lchip, void* p_policer, uint32* p_stats_ptr)
{
    uint32 cmd = 0;
    uint32 value = 0;
    sys_stats_param_t sts_param;
    sys_qos_policer_t* p_sys_policer = (sys_qos_policer_t*)p_policer;

    CTC_PTR_VALID_CHECK(p_sys_policer);
    CTC_PTR_VALID_CHECK(p_stats_ptr);

    if (p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM &&
        CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0))
    {
        switch(p_sys_policer->policer_lvl)
        {
        case SYS_QOS_POLICER_IPE_POLICING_0:
            if(p_sys_policer->type == CTC_QOS_POLICER_TYPE_COPP)
            {
                cmd = DRV_IOW(DsIpeCoPPConfig_t, DsIpeCoPPConfig_g_1_statsPtr_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, p_sys_policer->policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &value));
            }
            break;

        case SYS_QOS_POLICER_EPE_POLICING_0:
            if(p_sys_policer->type == CTC_QOS_POLICER_TYPE_COPP)
            {
                cmd = DRV_IOW(DsEpeCoPPConfig_t, DsEpeCoPPConfig_g_1_statsPtr_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, p_sys_policer->policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &value));
            }
            break;
        default:
            break;
        }
    }
    else if(p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM &&
        !CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0))
    {
        switch(p_sys_policer->policer_lvl)
        {
        case SYS_QOS_POLICER_IPE_POLICING_0:
            if(p_sys_policer->type == CTC_QOS_POLICER_TYPE_COPP)
            {
                cmd = DRV_IOW(DsIpeCoPPConfig_t, DsIpeCoPPConfig_g_0_statsPtr_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, p_sys_policer->policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &value));
            }
            break;
        case SYS_QOS_POLICER_EPE_POLICING_0:
            if(p_sys_policer->type == CTC_QOS_POLICER_TYPE_COPP)
            {
                cmd = DRV_IOW(DsEpeCoPPConfig_t, DsEpeCoPPConfig_g_0_statsPtr_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, p_sys_policer->policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &value));
            }
            break;
        default:
            break;
        }
    }

    sal_memset(&sts_param, 0, sizeof(sts_param));
    sts_param.dir = (p_sys_policer->policer_lvl) / 2;
    sts_param.num = p_sys_policer->stats_num;
    sts_param.type = (((p_sys_policer->policer_lvl) % 2) ? SYS_STATS_TYPE_POLICER1: SYS_STATS_TYPE_POLICER0);
    sts_param.ptr = *p_stats_ptr;
    CTC_ERROR_RETURN(sys_usw_flow_stats_free_statsptr(lchip, &sts_param));
    *p_stats_ptr = 0;

    return CTC_E_NONE;
}

int32
sys_tmm_qos_get_policer_param(uint8 lchip, void* p_param, void* p_policer)
{
    uint32 cmd1 = 0;
    uint32 cmd2 = 0;
    uint32 table_index = 0;
    uint32 tbl_id1 = 0;
    uint32 tbl_id2 = 0;
    uint32 fld_id[6] = {0};
    uint32 value = 0;
    ctc_qos_policer_t* p_policer_param = (ctc_qos_policer_t*)p_param;
    sys_qos_policer_t* p_sys_policer = (sys_qos_policer_t*)p_policer;
    DsIpePolicing0Config_m ipe_policer0_cfg;
    DsIpePolicing0ConfigE_m ipe_policer0_cfgE;


    table_index = p_sys_policer->policer_ptr + p_policer_param->hbwp.cos_index;


    switch(p_sys_policer->policer_lvl)
    {
        case SYS_QOS_POLICER_IPE_POLICING_0:
            tbl_id1 = DsIpePolicing0Config_t;
            tbl_id2 = DsIpePolicing0ConfigE_t;
            fld_id[0] = DsIpePolicing0Config_colorBlindMode_f;
            fld_id[1] = DsIpePolicing0ConfigE_conf_couplingFlag_f;
            fld_id[2] = DsIpePolicing0ConfigE_conf_layer3LengthEn_f;
            fld_id[3] = DsIpePolicing0Config_statsMode_f;
            fld_id[4] = DsIpePolicing0ConfigE_conf_envelopeEn_f;
            fld_id[5] = DsIpePolicing0ConfigE_conf_couplingFlagTotal_f;
            break;

        case SYS_QOS_POLICER_IPE_POLICING_1:
            tbl_id1 = DsIpePolicing1Config_t;
            tbl_id2 = DsIpePolicing1ConfigE_t;
            fld_id[0] = DsIpePolicing1Config_colorBlindMode_f;
            fld_id[1] = DsIpePolicing1ConfigE_conf_couplingFlag_f;
            fld_id[2] = DsIpePolicing1ConfigE_conf_layer3LengthEn_f;
            fld_id[3] = DsIpePolicing1Config_statsMode_f;
            fld_id[4] = DsIpePolicing1ConfigE_conf_envelopeEn_f;
            fld_id[5] = DsIpePolicing1ConfigE_conf_couplingFlagTotal_f;
            break;

        case SYS_QOS_POLICER_EPE_POLICING_0:
            tbl_id1 = DsEpePolicing0Config_t;
            tbl_id2 = DsEpePolicing0ConfigE_t;
            fld_id[0] = DsEpePolicing0Config_colorBlindMode_f;
            fld_id[1] = DsEpePolicing0ConfigE_conf_couplingFlag_f;
            fld_id[2] = DsEpePolicing0ConfigE_conf_layer3LengthEn_f;
            fld_id[3] = DsEpePolicing0Config_statsMode_f;
            fld_id[4] = DsEpePolicing0ConfigE_conf_envelopeEn_f;
            fld_id[5] = DsEpePolicing0ConfigE_conf_couplingFlagTotal_f;
            break;

        case SYS_QOS_POLICER_EPE_POLICING_1:
            tbl_id1 = DsEpePolicing1Config_t;
            tbl_id2 = DsEpePolicing1ConfigE_t;
            fld_id[0] = DsEpePolicing1Config_colorBlindMode_f;
            fld_id[1] = DsEpePolicing1ConfigE_conf_couplingFlag_f;
            fld_id[2] = DsEpePolicing1ConfigE_conf_layer3LengthEn_f;
            fld_id[3] = DsEpePolicing1Config_statsMode_f;
            fld_id[4] = DsEpePolicing1ConfigE_conf_envelopeEn_f;
            fld_id[5] = DsEpePolicing1ConfigE_conf_couplingFlagTotal_f;
            break;

        default:
            return CTC_E_INVALID_PARAM;
    }
    cmd1 = DRV_IOR(tbl_id1, DRV_ENTRY_FLAG);
    cmd2 = DRV_IOR(tbl_id2, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd1), &ipe_policer0_cfg));
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd2), &ipe_policer0_cfgE));
    drv_get_field(lchip, tbl_id1, fld_id[0], &ipe_policer0_cfg, &value);
    p_policer_param->policer.is_color_aware = value ? 0 : 1;
    drv_get_field(lchip, tbl_id2, fld_id[1],&ipe_policer0_cfgE, &value);
    p_policer_param->policer.cf_en = value;
    drv_get_field(lchip, tbl_id2, fld_id[2],&ipe_policer0_cfgE, &value);
    p_policer_param->policer.use_l3_length = value;
    drv_get_field(lchip, tbl_id1, fld_id[3],&ipe_policer0_cfg, &value);
    p_policer_param->policer.stats_mode = value ? 0 : 1;

    CTC_ERROR_RETURN(_sys_usw_qos_get_policer_rate_and_threshold(lchip, p_policer_param->hbwp.cos_index, p_policer_param, p_sys_policer));
    drv_get_field(lchip, tbl_id2, fld_id[4],&ipe_policer0_cfgE, &value);
    p_policer_param->hbwp.sf_en = value;
    drv_get_field(lchip, tbl_id2, fld_id[5],&ipe_policer0_cfgE, &value);
    p_policer_param->hbwp.cf_total_dis = !value;

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_alloc_offset(uint8 lchip, void* p_policer)
{
    uint8 count_type = 0;
    uint32 offset = 0;
    uint32 policer_num = 0;
    sys_qos_policer_t* p_sys_policer = (sys_qos_policer_t*)p_policer;

    SYS_QOS_POLICER_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    if (p_sys_policer->sys_policer_type == SYS_QOS_POLICER_TYPE_SVC && DRV_FROM_TMM(lchip))
    {

        if (CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_VLAN_STORMCTL))
        {
            offset = p_sys_policer->id << 2;
        }
        else
        {
            sys_usw_opf_t opf;
            sal_memset(&opf, 0, sizeof(sys_usw_opf_t));
            opf.pool_index = (CTC_QOS_POLICER_TYPE_SERVICE == p_sys_policer->type) ? 2 : (CTC_QOS_POLICER_TYPE_VLAN == p_sys_policer->type ? 1 : 0);
            opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_svc_config;
            p_sys_policer->entry_size = 1;
            CTC_ERROR_RETURN(sys_usw_opf_alloc_offset(lchip, &opf, p_sys_policer->entry_size, &offset));

            if (CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_FID_STORMCTL))
            {
                offset = offset << 2;
            }
        }
        p_sys_policer->policer_ptr = p_usw_qos_policer_master[lchip]->svc_policer_base + offset;

        p_usw_qos_policer_master[lchip]->policer_count[SYS_POLICER_CNT_SVC_POLICER] += 1;

    }
    else
    {
        sys_usw_opf_t opf;
        sal_memset(&opf, 0, sizeof(sys_usw_opf_t));
        opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_policer_config;
        CTC_ERROR_RETURN(_sys_tmm_qos_policer_get_opf_index(lchip, p_sys_policer, &opf.pool_index));

        if (p_sys_policer->hbwp_en)
        {
            p_sys_policer->entry_size = MCHIP_CAP(SYS_CAP_QOS_POLICER_MAX_COS_LEVEL) ;

            CTC_MAX_VALUE_CHECK(p_sys_policer->cos_index, p_sys_policer->entry_size - 1);
            CTC_BIT_SET(p_sys_policer->cos_bitmap, p_sys_policer->cos_index);
            /*D2 asic must 4 mulitple,TM&TMM is 8*/
        }
        else
        {
            p_sys_policer->entry_size = 1;
        }
        opf.multiple =  p_sys_policer->entry_size;

        if(DRV_FROM_AT(lchip))
        {
            policer_num = p_usw_qos_policer_master[lchip]->policer_count[SYS_POLICER_CNT_PORT_POLICER]
                        + p_usw_qos_policer_master[lchip]->policer_count[SYS_POLICER_CNT_VLAN_POLICER]
                        + p_usw_qos_policer_master[lchip]->policer_count[SYS_POLICER_CNT_FLOW_POLICER]
                        + p_usw_qos_policer_master[lchip]->policer_count[SYS_POLICER_CNT_CHAN_POLICER]
                        + p_sys_policer->entry_size;
            if(policer_num > MCHIP_CAP(SYS_CAP_QOS_POLICER_POLICER_NUM))
            {
                return CTC_E_NO_RESOURCE;
            }
        }

        //CTC_ERROR_RETURN(_sys_tmm_qos_policer_alloc_check(lchip, p_sys_policer));
        CTC_ERROR_RETURN(sys_usw_opf_alloc_offset(lchip, &opf, p_sys_policer->entry_size, &offset));

        p_sys_policer->policer_ptr = offset;

        p_sys_policer->split_en = (p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM);

        switch(p_sys_policer->type)
        {
            case CTC_QOS_POLICER_TYPE_PORT:
                count_type = SYS_POLICER_CNT_PORT_POLICER;
                break;
            case CTC_QOS_POLICER_TYPE_VLAN:
                count_type = SYS_POLICER_CNT_VLAN_POLICER;
                break;
            case CTC_QOS_POLICER_TYPE_FLOW:
                count_type = SYS_POLICER_CNT_FLOW_POLICER;
                break;
            case CTC_QOS_POLICER_TYPE_CHAN:
                count_type = SYS_POLICER_CNT_CHAN_POLICER;
                break;
            default:
                /*error*/
                break;
        }

        p_usw_qos_policer_master[lchip]->policer_count[count_type] += p_sys_policer->entry_size;

    }

    SYS_QOS_POLICER_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "Alloc policer offset = %d\n", offset);

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_free_offset(uint8 lchip, void* p_policer)
{
    uint8 count_type = 0;
    uint32 offset = 0;
    sys_qos_policer_t* p_sys_policer = (sys_qos_policer_t*)p_policer;

    if (p_sys_policer->sys_policer_type == SYS_QOS_POLICER_TYPE_SVC && DRV_FROM_TMM(lchip))
    {
        if (!CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_VLAN_STORMCTL))
        {
            sys_usw_opf_t opf;
            offset = p_sys_policer->policer_ptr - p_usw_qos_policer_master[lchip]->svc_policer_base;
            sal_memset(&opf, 0, sizeof(sys_usw_opf_t));
            opf.pool_index = (CTC_QOS_POLICER_TYPE_SERVICE == p_sys_policer->type) ? 2 : (CTC_QOS_POLICER_TYPE_VLAN == p_sys_policer->type ? 1 : 0);
            opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_svc_config;
            p_sys_policer->policer_ptr = 0;

            if (CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_FID_STORMCTL))
            {
                offset = offset >> 2;
            }
            CTC_ERROR_RETURN(sys_usw_opf_free_offset(lchip, &opf, p_sys_policer->entry_size, offset));
        }

        if(p_usw_qos_policer_master[lchip]->policer_count[SYS_POLICER_CNT_SVC_POLICER] >= 1)
        {
            p_usw_qos_policer_master[lchip]->policer_count[SYS_POLICER_CNT_SVC_POLICER] -= 1;
        }
    }
    else
    {
        sys_usw_opf_t opf;
        offset = p_sys_policer->policer_ptr;
        sal_memset(&opf, 0, sizeof(sys_usw_opf_t));
        opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_policer_config;
        CTC_ERROR_RETURN(_sys_tmm_qos_policer_get_opf_index(lchip, p_sys_policer, &opf.pool_index));
        p_sys_policer->policer_ptr = 0;
        if (p_sys_policer->entry_size)
        {
            CTC_ERROR_RETURN(sys_usw_opf_free_offset(lchip, &opf, p_sys_policer->entry_size, offset));
        }

        switch(p_sys_policer->type)
        {
            case CTC_QOS_POLICER_TYPE_PORT:
                count_type = SYS_POLICER_CNT_PORT_POLICER;
                break;
            case CTC_QOS_POLICER_TYPE_VLAN:
                count_type = SYS_POLICER_CNT_VLAN_POLICER;
                break;
            case CTC_QOS_POLICER_TYPE_FLOW:
                count_type = SYS_POLICER_CNT_FLOW_POLICER;
                break;
            case CTC_QOS_POLICER_TYPE_CHAN:
                count_type = SYS_POLICER_CNT_CHAN_POLICER;
                break;
            default:
                /*error*/
                break;
        }

        if (p_usw_qos_policer_master[lchip]->policer_count[count_type] >= p_sys_policer->entry_size)
        {
            p_usw_qos_policer_master[lchip]->policer_count[count_type] -= p_sys_policer->entry_size;
        }
    }

    SYS_QOS_POLICER_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "Free policer offset = %d\n", offset);

    return CTC_E_NONE;
}

/**
 @brief Write policer to asic.
*/
int32
sys_tmm_qos_policer_add_to_asic(uint8 lchip, void* p_policer, void* p_profX,
                                                   void* p_profY, void*  p_action_profile)
{
    uint32 cmd         = 0;
    uint8 color_blind_mode  = 0;
     /*uint8 color_drop  = 0;*/
    uint8 use_len  = 0;
    uint8 cf  = 0;
    uint32 cir_cnt = 0;
    uint32 pir_cnt = 0;
    uint8 policer_mode = 0;
    uint8 splitEn =0;
    uint8 is_pps = 0;
    uint8 statsMode = 0;
    uint32 countX_tbl_id = 0;
    uint32 countY_tbl_id = 0;
    uint32 table_index = 0;
    uint32 value = 0;
    uint8 share_mode = 0;
    uint16 stats_ptr = 0;
    sys_qos_policer_t* p_sys_policer = (sys_qos_policer_t*)p_policer;
    sys_qos_policer_profile_t* p_sys_profX = (sys_qos_policer_profile_t*)p_profX;
    sys_qos_policer_profile_t* p_sys_profY = (sys_qos_policer_profile_t*)p_profY;
    sys_qos_policer_action_t*  p_sys_action_profile = (sys_qos_policer_action_t*)p_action_profile;

    DsIpePolicing0CountX_m policer_countX;
    DsIpePolicing0CountY_m policer_countY;
    DsIpePolicing0Config_m ipe_policer0_cfg;
    DsIpePolicing1Config_m ipe_policer1_cfg;
    DsEpePolicing0Config_m epe_policer0_cfg;
    DsEpePolicing1Config_m epe_policer1_cfg;
    DsIpePolicing0ConfigE_m ipe_policer0_cfgE;
    DsIpePolicing1ConfigE_m ipe_policer1_cfgE;
    DsEpePolicing0ConfigE_m epe_policer0_cfgE;
    DsEpePolicing1ConfigE_m epe_policer1_cfgE;


    SYS_QOS_POLICER_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);


    sal_memset(&ipe_policer0_cfg,0,sizeof(DsIpePolicing0Config_m));
    sal_memset(&ipe_policer1_cfg,0,sizeof(DsIpePolicing1Config_m));
    sal_memset(&epe_policer0_cfg,0,sizeof(DsEpePolicing0Config_m));
    sal_memset(&epe_policer1_cfg,0,sizeof(DsEpePolicing1Config_m));
    sal_memset(&ipe_policer0_cfgE,0,sizeof(DsIpePolicing0ConfigE_m));
    sal_memset(&ipe_policer1_cfgE,0,sizeof(DsIpePolicing1ConfigE_m));
    sal_memset(&epe_policer0_cfgE,0,sizeof(DsEpePolicing0ConfigE_m));
    sal_memset(&epe_policer1_cfgE,0,sizeof(DsEpePolicing1ConfigE_m));


    is_pps = p_sys_policer->is_pps;
    table_index = p_sys_policer->policer_ptr + p_sys_policer->cos_index;
    color_blind_mode = p_sys_policer->is_color_aware ? 0 : 1;
    use_len    = p_sys_policer->use_l3_length ? 1 : 0;
    policer_mode = p_sys_policer->policer_mode;
    splitEn = (p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM) ? 1 : 0;
    statsMode = !(p_sys_policer->stats_mode);

    if (p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_MEF_BWP ||
        p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_RFC4115)
    {
        cf = p_sys_policer->cf_en ? 1 : 0;
        policer_mode = CTC_QOS_POLICER_MODE_RFC4115;
    }
    else if (p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_RFC2697)
    {
        cf = 1;
    }

    if (p_sys_profX)
    {
        cir_cnt = (p_sys_profX->threshold << p_sys_profX->thresholdShift);
    }
    if (p_sys_profY)
    {
        pir_cnt = (p_sys_profY->threshold << p_sys_profY->thresholdShift);
    }

    if (cir_cnt > 0x1FFFFFF)
    {
        cir_cnt = SYS_QOS_POLICER_MAX_COUNT;
    }
    if (pir_cnt > 0x1FFFFFF)
    {
        pir_cnt = SYS_QOS_POLICER_MAX_COUNT;
    }
    if(p_sys_policer->color_merge_mode == CTC_QOS_POLICER_COLOR_MERGE_AND)
    {
        share_mode = 1;
    }
    else if(p_sys_policer->color_merge_mode == CTC_QOS_POLICER_COLOR_MERGE_OR)
    {
        share_mode = 0;
    }
    else
    {
        share_mode = 2;
    }

    /*golod config*/
    //discard_color = CTC_QOS_COLOR_RED;
    stats_ptr = p_sys_policer->stats_ptr[p_sys_policer->cos_index];

    switch(p_sys_policer->policer_lvl)
    {
    case SYS_QOS_POLICER_IPE_POLICING_0:
         /*config DsIpePolicing0Config*/
         cmd = DRV_IOR(DsIpePolicing0Config_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &ipe_policer0_cfg));
         SetDsIpePolicing0Config(V, actionProfId_f, &ipe_policer0_cfg, policer_mode-1);
         SetDsIpePolicing0Config(V, colorBlindMode_f, &ipe_policer0_cfg, color_blind_mode);
         SetDsIpePolicing0Config(V, discardColor_f, &ipe_policer0_cfg, p_sys_policer->drop_color);
         SetDsIpePolicing0Config(V, statsMode_f, &ipe_policer0_cfg, statsMode);
         SetDsIpePolicing0Config(V, policyProfId_f, &ipe_policer0_cfg, p_sys_action_profile->profile_id);
         SetDsIpePolicing0Config(V, statsPtr_f, &ipe_policer0_cfg, stats_ptr);
         SetDsIpePolicing0Config(V, shareMode_f, &ipe_policer0_cfg, share_mode);
         SetDsIpePolicing0Config(V, splitEn_f, &ipe_policer0_cfg, splitEn);
         cmd = DRV_IOW(DsIpePolicing0Config_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &ipe_policer0_cfg));

         /*config DsIpePolicing0ConfigE*/
         cmd = DRV_IOR(DsIpePolicing0ConfigE_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &ipe_policer0_cfgE));
         SetDsIpePolicing0ConfigE(V, conf_couplingFlag_f, &ipe_policer0_cfgE, cf);
         SetDsIpePolicing0ConfigE(V, conf_layer3LengthEn_f, &ipe_policer0_cfgE, use_len);
         if (p_sys_profX)
         {
             SetDsIpePolicing0ConfigE(V, profIdX_f, &ipe_policer0_cfgE, p_sys_profX->profile_id);
             SetDsIpePolicing0ConfigE(V, conf_ppsModeX_f, &ipe_policer0_cfgE, is_pps);

             countX_tbl_id = DsIpePolicing0CountX_t;
         }
         if (p_sys_profY)
         {
             SetDsIpePolicing0ConfigE(V, profIdY_f, &ipe_policer0_cfgE, p_sys_profY->profile_id);
             SetDsIpePolicing0ConfigE(V, conf_ppsModeY_f, &ipe_policer0_cfgE, is_pps);

             countY_tbl_id = DsIpePolicing0CountY_t;
         }
         if (p_sys_policer->hbwp_en)
         {
             SetDsIpePolicing0ConfigE(V, conf_envelopeEn_f, &ipe_policer0_cfgE, p_sys_policer->sf_en);
             SetDsIpePolicing0ConfigE(V, conf_couplingFlagTotal_f, &ipe_policer0_cfgE, !p_sys_policer->cf_total_dis);
         }
         cmd = DRV_IOW(DsIpePolicing0ConfigE_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &ipe_policer0_cfgE));
         break;

    case SYS_QOS_POLICER_IPE_POLICING_1:
         /*config DsIpePolicing1Config*/
         cmd = DRV_IOR(DsIpePolicing1Config_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &ipe_policer1_cfg));
         SetDsIpePolicing1Config(V, actionProfId_f, &ipe_policer1_cfg, policer_mode-1);
         SetDsIpePolicing1Config(V, colorBlindMode_f, &ipe_policer1_cfg, color_blind_mode);
         SetDsIpePolicing1Config(V, discardColor_f, &ipe_policer1_cfg, p_sys_policer->drop_color);
         SetDsIpePolicing1Config(V, statsMode_f, &ipe_policer1_cfg, statsMode);
         SetDsIpePolicing1Config(V, policyProfId_f, &ipe_policer1_cfg, p_sys_action_profile->profile_id);
         SetDsIpePolicing1Config(V, statsPtr_f, &ipe_policer1_cfg, stats_ptr);
         SetDsIpePolicing1Config(V, splitEn_f, &ipe_policer1_cfg, splitEn);
         cmd = DRV_IOW(DsIpePolicing1Config_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &ipe_policer1_cfg));

         /*config DsIpePolicing1ConfigE*/
         cmd = DRV_IOR(DsIpePolicing1ConfigE_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &ipe_policer1_cfgE));
         SetDsIpePolicing1ConfigE(V, conf_couplingFlag_f, &ipe_policer1_cfgE, cf);
         SetDsIpePolicing1ConfigE(V, conf_layer3LengthEn_f, &ipe_policer1_cfgE, use_len);
         if (p_sys_profX)
         {
             SetDsIpePolicing1ConfigE(V, profIdX_f, &ipe_policer1_cfgE, p_sys_profX->profile_id);
             SetDsIpePolicing1ConfigE(V, conf_ppsModeX_f, &ipe_policer1_cfgE, is_pps);

             countX_tbl_id = DsIpePolicing1CountX_t;
         }
         if (p_sys_profY)
         {
             SetDsIpePolicing1ConfigE(V, profIdY_f, &ipe_policer1_cfgE, p_sys_profY->profile_id);
             SetDsIpePolicing1ConfigE(V, conf_ppsModeY_f, &ipe_policer1_cfgE, is_pps);

             countY_tbl_id = DsIpePolicing1CountY_t;
         }
         if (p_sys_policer->hbwp_en)
         {
             SetDsIpePolicing1ConfigE(V, conf_envelopeEn_f, &ipe_policer1_cfgE, p_sys_policer->sf_en);
             SetDsIpePolicing1ConfigE(V, conf_couplingFlagTotal_f, &ipe_policer1_cfgE, !p_sys_policer->cf_total_dis);
         }
         cmd = DRV_IOW(DsIpePolicing1ConfigE_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &ipe_policer1_cfgE));
         break;

    case SYS_QOS_POLICER_EPE_POLICING_0:
        /*config DsEpePolicing0Config*/
         cmd = DRV_IOR(DsEpePolicing0Config_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &epe_policer0_cfg));
         SetDsEpePolicing0Config(V, actionProfId_f, &epe_policer0_cfg, policer_mode - 1);
         SetDsEpePolicing0Config(V, colorBlindMode_f, &epe_policer0_cfg, color_blind_mode);
         SetDsEpePolicing0Config(V, discardColor_f, &epe_policer0_cfg, p_sys_policer->drop_color);
         SetDsEpePolicing0Config(V, statsMode_f, &epe_policer0_cfg, statsMode);
         SetDsEpePolicing0Config(V, policyProfId_f, &epe_policer0_cfg, p_sys_action_profile->profile_id);
         SetDsEpePolicing0Config(V, statsPtr_f, &epe_policer0_cfg, stats_ptr);
         SetDsEpePolicing0Config(V, shareMode_f, &epe_policer0_cfg, share_mode);
         SetDsEpePolicing0Config(V, splitEn_f, &epe_policer0_cfg, splitEn);
         cmd = DRV_IOW(DsEpePolicing0Config_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &epe_policer0_cfg));

         /*config DsEpePolicing0ConfigE*/
         cmd = DRV_IOR(DsEpePolicing0ConfigE_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &epe_policer0_cfgE));
         SetDsEpePolicing0ConfigE(V, conf_couplingFlag_f, &epe_policer0_cfgE, cf);
         SetDsEpePolicing0ConfigE(V, conf_layer3LengthEn_f, &epe_policer0_cfgE, use_len);
         if (p_sys_profX)
         {
             SetDsEpePolicing0ConfigE(V, profIdX_f, &epe_policer0_cfgE, p_sys_profX->profile_id);
             SetDsEpePolicing0ConfigE(V, conf_ppsModeX_f, &epe_policer0_cfgE, is_pps);

             countX_tbl_id = DsEpePolicing0CountX_t;
         }
         if (p_sys_profY)
         {
             SetDsEpePolicing0ConfigE(V, profIdY_f, &epe_policer0_cfgE, p_sys_profY->profile_id);
             SetDsEpePolicing0ConfigE(V, conf_ppsModeY_f, &epe_policer0_cfgE, is_pps);

             countY_tbl_id = DsEpePolicing0CountY_t;
         }
         if (p_sys_policer->hbwp_en)
         {
             SetDsEpePolicing0ConfigE(V, conf_envelopeEn_f, &epe_policer0_cfgE, p_sys_policer->sf_en);
             SetDsEpePolicing0ConfigE(V, conf_couplingFlagTotal_f, &epe_policer0_cfgE, !p_sys_policer->cf_total_dis);
         }
         cmd = DRV_IOW(DsEpePolicing0ConfigE_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &epe_policer0_cfgE));
         break;

    case SYS_QOS_POLICER_EPE_POLICING_1:
        /*config DsEpePolicing1Config*/
         cmd = DRV_IOR(DsEpePolicing1Config_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &epe_policer1_cfg));
         SetDsEpePolicing1Config(V, actionProfId_f, &epe_policer1_cfg, policer_mode - 1);
         SetDsEpePolicing1Config(V, colorBlindMode_f, &epe_policer1_cfg, color_blind_mode);
         SetDsEpePolicing1Config(V, discardColor_f, &epe_policer1_cfg, p_sys_policer->drop_color);
         SetDsEpePolicing1Config(V, statsMode_f, &epe_policer1_cfg, statsMode);
         SetDsEpePolicing1Config(V, policyProfId_f, &epe_policer1_cfg, p_sys_action_profile->profile_id);
         SetDsEpePolicing1Config(V, statsPtr_f, &epe_policer1_cfg, stats_ptr);
         SetDsEpePolicing1Config(V, splitEn_f, &epe_policer1_cfg, splitEn);
         cmd = DRV_IOW(DsEpePolicing1Config_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &epe_policer1_cfg));

         /*config DsEpePolicing1ConfigE*/
         cmd = DRV_IOR(DsEpePolicing1ConfigE_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &epe_policer1_cfgE));
         SetDsEpePolicing1ConfigE(V, conf_couplingFlag_f, &epe_policer1_cfgE, cf);
         SetDsEpePolicing1ConfigE(V, conf_layer3LengthEn_f, &epe_policer1_cfgE, use_len);
         if (p_sys_profX)
         {
             SetDsEpePolicing1ConfigE(V, profIdX_f, &epe_policer1_cfgE, p_sys_profX->profile_id);
             SetDsEpePolicing1ConfigE(V, conf_ppsModeX_f, &epe_policer1_cfgE, is_pps);

             countX_tbl_id = DsEpePolicing1CountX_t;
         }
         if (p_sys_profY)
         {
             SetDsEpePolicing1ConfigE(V, profIdY_f, &epe_policer1_cfgE, p_sys_profY->profile_id);
             SetDsEpePolicing1ConfigE(V, conf_ppsModeY_f, &epe_policer1_cfgE, is_pps);

             countY_tbl_id = DsEpePolicing1CountY_t;
         }

         if (p_sys_policer->hbwp_en)
         {
             SetDsEpePolicing1ConfigE(V, conf_envelopeEn_f, &epe_policer1_cfgE, p_sys_policer->sf_en);
             SetDsEpePolicing1ConfigE(V, conf_couplingFlagTotal_f, &epe_policer1_cfgE, !p_sys_policer->cf_total_dis);
         }
         cmd = DRV_IOW(DsEpePolicing1ConfigE_t, DRV_ENTRY_FLAG);
         CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &epe_policer1_cfgE));
         break;

    default:
        return CTC_E_INVALID_PARAM;
    }

    if (0 != countX_tbl_id)
    {
        cmd = DRV_IOR(countX_tbl_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &policer_countX));
        SetDsIpePolicing0CountX(V, count_f, &policer_countX, cir_cnt);
        SetDsIpePolicing0CountX(V, sign_f, &policer_countX, value);
        cmd = DRV_IOW(countX_tbl_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &policer_countX));
    }
    if(0 != countY_tbl_id)
    {
        cmd = DRV_IOR(countY_tbl_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &policer_countY));
        SetDsIpePolicing0CountY(V, count_f, &policer_countY, pir_cnt);
        SetDsIpePolicing0CountY(V, sign_f, &policer_countY, value);
        cmd = DRV_IOW(countY_tbl_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &policer_countY));
    }

    SYS_QOS_POLICER_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "Add to asic Policer config,  index = %d\n",
                             table_index);

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_remove_from_asic(uint8 lchip, void* p_policer)
{
    sys_qos_policer_t* p_sys_policer = (sys_qos_policer_t*)p_policer;
    SYS_QOS_POLICER_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    /*For normal policer, only remover SW DB,
    For HBWP, need reset the profie to 0 for no rate share*/
    if (p_sys_policer->hbwp_en || p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
    {
        uint32 cmd = 0;
        uint32 field_val = 0;
        uint32 table_index = 0;

        table_index = p_sys_policer->policer_ptr + p_sys_policer->cos_index;
        switch(p_sys_policer->policer_lvl)
        {
        case SYS_QOS_POLICER_IPE_POLICING_0:
            if((p_sys_policer->type == CTC_QOS_POLICER_TYPE_COPP)&&
                CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0))
            {
                cmd = DRV_IOW(DsIpeCoPPConfigE_t, DsIpeCoPPConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index >> 1, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else if((p_sys_policer->type == CTC_QOS_POLICER_TYPE_COPP)&&
                !CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0))
            {
                cmd = DRV_IOW(DsIpeCoPPConfigE_t, DsIpeCoPPConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index >> 1, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else if(CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0)&&
                p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
            {
                cmd = DRV_IOW(DsIpePolicing0ConfigE_t, DsIpePolicing0ConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else if(!CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0)&&
                p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
            {
                cmd = DRV_IOW(DsIpePolicing0ConfigE_t, DsIpePolicing0ConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else
            {
                cmd = DRV_IOW(DsIpePolicing0ConfigE_t, DsIpePolicing0ConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
                cmd = DRV_IOW(DsIpePolicing0ConfigE_t, DsIpePolicing0ConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            break;

        case SYS_QOS_POLICER_IPE_POLICING_1:
            if(CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0)&&
                p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
            {
                cmd = DRV_IOW(DsIpePolicing1ConfigE_t, DsIpePolicing1ConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else if(!CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0)&&
            p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
            {
                cmd = DRV_IOW(DsIpePolicing1ConfigE_t, DsIpePolicing1ConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else
            {
                cmd = DRV_IOW(DsIpePolicing1ConfigE_t, DsIpePolicing1ConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
                cmd = DRV_IOW(DsIpePolicing1ConfigE_t, DsIpePolicing1ConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            break;

        case SYS_QOS_POLICER_EPE_POLICING_0:
            if((p_sys_policer->type == CTC_QOS_POLICER_TYPE_COPP)&&
                CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0))
            {
                cmd = DRV_IOW(DsEpeCoPPConfigE_t, DsEpeCoPPConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index >> 1, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else if((p_sys_policer->type == CTC_QOS_POLICER_TYPE_COPP)&&
                !CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0))
            {
                cmd = DRV_IOW(DsEpeCoPPConfigE_t, DsEpeCoPPConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index >> 1, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else if(CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0)&&
                p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
            {
                cmd = DRV_IOW(DsEpePolicing0ConfigE_t, DsEpePolicing0ConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else if(!CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0)&&
                p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
            {
                cmd = DRV_IOW(DsEpePolicing0ConfigE_t, DsEpePolicing0ConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else
            {
                cmd = DRV_IOW(DsEpePolicing0ConfigE_t, DsEpePolicing0ConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
                cmd = DRV_IOW(DsEpePolicing0ConfigE_t, DsEpePolicing0ConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            break;

        case SYS_QOS_POLICER_EPE_POLICING_1:
            if(CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0)&&
                p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
            {
                cmd = DRV_IOW(DsEpePolicing1ConfigE_t, DsEpePolicing1ConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else if(!CTC_IS_BIT_SET(p_sys_policer->policer_ptr, 0)&&
                p_sys_policer->policer_mode == CTC_QOS_POLICER_MODE_STBM)
            {
                cmd = DRV_IOW(DsEpePolicing1ConfigE_t, DsEpePolicing1ConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            else
            {
                cmd = DRV_IOW(DsEpePolicing1ConfigE_t, DsEpePolicing1ConfigE_profIdX_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
                cmd = DRV_IOW(DsEpePolicing1ConfigE_t, DsEpePolicing1ConfigE_profIdY_f);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, table_index, DRV_CMD_PP_EN(cmd), &field_val));
            }
            break;

        default:
            return CTC_E_INVALID_PARAM;
        }
    }

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_copp_add_to_asic(uint8 lchip, void* p_policer, void* p_prof)
{
    uint32 cmd         = 0;
    uint32 bucket_cnt = 0;
    uint16 policer_ptr = 0;
    uint8  is_pps = 0;
    uint8  step = 0;
    uint32 value = 0;
    uint32 count_tbl_id = 0;
    sys_qos_policer_t* p_sys_policer = (sys_qos_policer_t*)p_policer;
    sys_qos_policer_copp_profile_t* p_sys_prof = (sys_qos_policer_copp_profile_t*)p_prof;

    DsIpeCoPPCountX_m copp_count;
    DsIpeCoPPConfig_m ipe_copp_cfg;
    DsEpeCoPPConfig_m epe_copp_cfg;
    DsIpeCoPPConfigE_m ipe_copp_cfgE;
    DsEpeCoPPConfigE_m epe_copp_cfgE;

    SYS_QOS_POLICER_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    CTC_PTR_VALID_CHECK(p_sys_prof);

    sal_memset(&ipe_copp_cfg,0,sizeof(DsIpeCoPPConfig_m));

    bucket_cnt = (p_sys_prof->threshold << p_sys_prof->thresholdShift);
    policer_ptr = p_sys_policer->policer_ptr;
    is_pps = p_sys_policer->is_pps;

    if (bucket_cnt > 0x1ffffff)
    {
        bucket_cnt = SYS_QOS_POLICER_MAX_COUNT;
    }

    switch(p_sys_policer->policer_lvl / 2)
    {
        case 0:
            /*config DsIpeCoPPConfig*/
            cmd = DRV_IOR(DsIpeCoPPConfig_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &ipe_copp_cfg));
            if(p_sys_prof->is_bucketY)
            {
                step = DsIpeCoPPConfig_g_1_statsMode_f - DsIpeCoPPConfig_g_0_statsMode_f;
            }
            SetDsIpeCoPPConfig(V, g_0_statsMode_f + step, &ipe_copp_cfg, !p_sys_policer->stats_mode);
            SetDsIpeCoPPConfig(V, g_0_statsPtr_f + step, &ipe_copp_cfg, p_sys_policer->stats_ptr[0]);
            cmd = DRV_IOW(DsIpeCoPPConfig_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &ipe_copp_cfg));

            /*config DsIpeCoPPConfigE*/
            cmd = DRV_IOR(DsIpeCoPPConfigE_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &ipe_copp_cfgE));
            if (p_sys_prof->is_bucketY)
            {

                SetDsIpeCoPPConfigE(V, profIdY_f, &ipe_copp_cfgE, p_sys_prof->profile_id);
                SetDsIpeCoPPConfigE(V, conf_ppsModeY_f, &ipe_copp_cfgE, is_pps);

                count_tbl_id = DsIpeCoPPCountY_t;
            }
            else
            {
                SetDsIpeCoPPConfigE(V, profIdX_f, &ipe_copp_cfgE, p_sys_prof->profile_id);
                SetDsIpeCoPPConfigE(V, conf_ppsModeX_f, &ipe_copp_cfgE, is_pps);

                count_tbl_id = DsIpeCoPPCountX_t;
            }
            cmd = DRV_IOW(DsIpeCoPPConfigE_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &ipe_copp_cfgE));
            break;
        case 1:
            /*config DsEpeCoPPConfig*/
            cmd = DRV_IOR(DsEpeCoPPConfig_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &epe_copp_cfg));
            if(p_sys_prof->is_bucketY)
            {
                step = DsIpeCoPPConfig_g_1_statsMode_f - DsIpeCoPPConfig_g_0_statsMode_f;
            }

            SetDsEpeCoPPConfig(V, g_0_statsMode_f + step, &epe_copp_cfg, !p_sys_policer->stats_mode);
            SetDsEpeCoPPConfig(V, g_0_statsPtr_f + step, &epe_copp_cfg, p_sys_policer->stats_ptr[0]);
            cmd = DRV_IOW(DsEpeCoPPConfig_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &epe_copp_cfg));

            /*config DsEpeCoPPConfigE*/
            cmd = DRV_IOR(DsEpeCoPPConfigE_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &epe_copp_cfgE));
            if (p_sys_prof->is_bucketY)
            {
                SetDsEpeCoPPConfigE(V, profIdY_f, &epe_copp_cfgE, p_sys_prof->profile_id);
                SetDsEpeCoPPConfigE(V, conf_ppsModeY_f, &epe_copp_cfgE, is_pps);

                count_tbl_id = DsEpeCoPPCountY_t;
            }
            else
            {
                SetDsEpeCoPPConfigE(V, profIdX_f, &epe_copp_cfgE, p_sys_prof->profile_id);
                SetDsEpeCoPPConfigE(V, conf_ppsModeX_f, &epe_copp_cfgE, is_pps);

                count_tbl_id = DsEpeCoPPCountX_t;
            }

            cmd = DRV_IOW(DsEpeCoPPConfigE_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &epe_copp_cfgE));
            break;
        default:
            return CTC_E_INVALID_PARAM;
    }

    cmd = DRV_IOR(count_tbl_id, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &copp_count));
    SetDsIpeCoPPCountX(V, count_f, &copp_count, bucket_cnt);
    SetDsIpeCoPPCountX(V, sign_f, &copp_count, value);
    cmd = DRV_IOW(count_tbl_id, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, policer_ptr >> 1, DRV_CMD_PP_EN(cmd), &copp_count));
    SYS_QOS_POLICER_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "Add to asic copp config,  index = %d\n",
                             policer_ptr >> 1);

    return CTC_E_NONE;
}



int32
sys_tmm_qos_policer_deinit_opf(uint8 lchip)
{
    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_svc_config);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_epe_copp_profile);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_copp_profile);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_copp_config);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_action_profile);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_epe_policer1_profile);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_epe_policer0_profile);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_ipe_policer1_profile);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_ipe_policer0_profile);

    (void)sys_usw_opf_deinit(lchip, p_usw_qos_policer_master[lchip]->opf_type_policer_config);

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_init_opf(uint8 lchip)
{
    int32  ret            = CTC_E_NONE;
    uint32 max_size       = 0;
    uint32 start_offset   = 0;
    uint32 end_offset     = 0;
    sys_usw_opf_t opf;
    uint8  loop           = 0;

    /*init policer config opf(IpePolicer0/IpePolicer1/EpePolicer0/EpePolicer1)*/
    start_offset = 1 +  p_usw_qos_policer_master[lchip]->igs_macro_policer_num + p_usw_qos_policer_master[lchip]->glb_policer_num[SYS_QOS_POLICER_IPE_POLICING_0];
    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_policer_config,
                                                                        10, "opf-policer-config"), ret, out);
    sys_usw_ftm_query_table_entry_num(lchip, DsIpePolicing0Config_t, &max_size);
    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_policer_config;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - start_offset),ret, out);

    start_offset = 1 +  p_usw_qos_policer_master[lchip]->glb_policer_num[SYS_QOS_POLICER_IPE_POLICING_1];
    sys_usw_ftm_query_table_entry_num(lchip, DsIpePolicing1Config_t, &max_size);
    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - start_offset ),ret, out);

    start_offset = 1 + p_usw_qos_policer_master[lchip]->egs_macro_policer_num + p_usw_qos_policer_master[lchip]->glb_policer_num[SYS_QOS_POLICER_EPE_POLICING_0];
    sys_usw_ftm_query_table_entry_num(lchip, DsEpePolicing0Config_t, &max_size);
    opf.pool_index = 2;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - start_offset), ret, out);

    start_offset =1 + p_usw_qos_policer_master[lchip]->glb_policer_num[SYS_QOS_POLICER_EPE_POLICING_1];
    sys_usw_ftm_query_table_entry_num(lchip, DsEpePolicing1Config_t, &max_size);
    opf.pool_index = 3;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - start_offset),ret, out);

    /*ingress group opf*/
    if(p_usw_qos_policer_master[lchip]->igs_macro_policer_num)
    {
        opf.pool_index = 4;
        CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, 1, p_usw_qos_policer_master[lchip]->igs_macro_policer_num), ret, out);
    }

    /*egress group opf*/
    if (p_usw_qos_policer_master[lchip]->egs_macro_policer_num)
    {
        opf.pool_index = 5;
        CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, 1, p_usw_qos_policer_master[lchip]->egs_macro_policer_num), ret, out);
    }

    if (DRV_FROM_AT(lchip))
    {
        /* policer global ptr, opf index is 6-9*/
        opf.pool_index = 5;
        for (loop = 0; loop < SYS_QOS_POLICER_LEVEL_MAX; loop++)
        {
            start_offset = 1 + (SYS_QOS_POLICER_IPE_POLICING_0 == loop ? p_usw_qos_policer_master[lchip]->igs_macro_policer_num :
                                (SYS_QOS_POLICER_EPE_POLICING_0 == loop ? p_usw_qos_policer_master[lchip]->egs_macro_policer_num : 0)
                               );
            opf.pool_index ++;
            if(0 == p_usw_qos_policer_master[lchip]->glb_policer_num[loop])
            {
                continue;
            }
            CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, p_usw_qos_policer_master[lchip]->glb_policer_num[loop]), ret, out);
        }
    }
    /*init IpePolicer0 profileX and profileY opf*/
    start_offset = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_ipe_policer0_profile,
                                                                        2, "opf-ipe_policer0-profile"), ret, out);
    sys_usw_ftm_query_table_entry_num(lchip, DsIpePolicing0ProfileX_t, &max_size);
    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_ipe_policer0_profile;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    sys_usw_ftm_query_table_entry_num(lchip, DsIpePolicing0ProfileY_t, &max_size);
    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    /*init IpePolicer1 profileX and profileY opf*/
    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_ipe_policer1_profile,
                                                                        2, "opf-ipe_policer1-profile"), ret, out);
    sys_usw_ftm_query_table_entry_num(lchip, DsIpePolicing1ProfileX_t, &max_size);
    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_ipe_policer1_profile;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    sys_usw_ftm_query_table_entry_num(lchip, DsIpePolicing1ProfileY_t, &max_size);
    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    /*init EpePolicer0 profileX and profileY opf*/
    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_epe_policer0_profile,
                                                                        2, "opf-epe_policer0-profile"), ret, out);
    sys_usw_ftm_query_table_entry_num(lchip, DsEpePolicing0ProfileX_t, &max_size);
    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_epe_policer0_profile;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1),ret, out);

    sys_usw_ftm_query_table_entry_num(lchip, DsEpePolicing0ProfileY_t, &max_size);
    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1),ret, out);

    /*init EpePolicer1 profileX and profileY opf*/
    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_epe_policer1_profile,
                                                                        2, "opf-epe_policer1-profile"), ret, out);
    sys_usw_ftm_query_table_entry_num(lchip, DsEpePolicing1ProfileX_t, &max_size);
    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_epe_policer1_profile;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    sys_usw_ftm_query_table_entry_num(lchip, DsEpePolicing1ProfileY_t, &max_size);
    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    /*init policer action opf*/
    start_offset = 1;
    sys_usw_ftm_query_table_entry_num(lchip, DsIpePolicingPolicy_t, &max_size);

    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_action_profile,
                                                                        2, "opf-action-profile"), ret, out);

    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_action_profile;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    /*init copp config opf*/
    start_offset = 1;
    sys_usw_ftm_query_table_entry_num(lchip, DsIpeCoPPConfig_t, &max_size);

    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_copp_config,
                                                                        2, "opf-copp-config"), ret, out);

    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_copp_config;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size * 2 - 1), ret, out);

    sys_usw_ftm_query_table_entry_num(lchip, DsEpeCoPPConfig_t, &max_size);

    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_copp_config;
    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size * 2 - 1), ret, out);

    /*init ipe copp profile opf*/
    start_offset = 1;
    sys_usw_ftm_query_table_entry_num(lchip, DsIpeCoPPProfileX_t, &max_size);

    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_copp_profile,
                                                                        2, "opf-copp-profile"), ret, out);

    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_copp_profile;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    /*init epe copp profile opf*/
    sys_usw_ftm_query_table_entry_num(lchip, DsEpeCoPPProfileX_t, &max_size);

    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_epe_copp_profile,
                                                                        2, "opf-epe-copp-profile"), ret, out);

    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_epe_copp_profile;
    opf.pool_index = 0;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    opf.pool_index = 1;
    CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, max_size - 1), ret, out);

    /*init svc config opf*/
    sys_usw_ftm_query_table_entry_num(lchip, DsSvcPolicingConfig_t, &max_size);

    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_qos_policer_master[lchip]->opf_type_svc_config,
                                                                    3, "opf-svc-config"), ret, out);
    start_offset = 1;
    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_svc_config;
    if (CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_PORT_POLICER))
    {
        opf.pool_index = 0;
        end_offset = MCHIP_CAP(SYS_CAP_PER_DP_CHANNEL_NUM) * MCHIP_CAP(SYS_CAP_DP_MAX_NUM);
        CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, end_offset), ret, out);
        start_offset += end_offset;
    }

    if (CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_VLAN_POLICER))
    {
        opf.pool_index = 1;
        end_offset = CTC_MAX_VLAN_ID - CTC_MIN_VLAN_ID + 1;
        CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, end_offset), ret, out);
        start_offset += end_offset;
    }

    opf.pool_index = 2;
    if (CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_FID_STORMCTL))
    {
        /*svc policer use for fid storm_ctl, resource need to be divided by base(4) */
        end_offset = max_size * 8 / 4 - start_offset;
        CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, end_offset), ret, out);
    }
    else if (CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_VLAN_STORMCTL))
    {
        /*svc policer use for vlan storm_ctl, svc policer ptr policer_ptr = vlan_id, no need opf manage*/
        end_offset = 0;
        CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, end_offset), ret, out);
    }
    else
    {
        /*svc policer use for svc self use all resource */
        end_offset = max_size*8 - start_offset;
        CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, start_offset, end_offset), ret, out);
    }

out:
    return ret;

}

int32
sys_tmm_qos_policer_spool_init(uint8 lchip)
{
    int32 ret = CTC_E_NONE;
    ctc_spool_t spool;

    /*create svc policer spool*/
    sal_memset(&spool, 0, sizeof(ctc_spool_t));
    spool.lchip = lchip;
    spool.block_num = 1;
    spool.block_size = SYS_QOS_POLICER_SVC_PROF_HASH_BLOCK_SIZE;
    spool.max_count = MCHIP_CAP(SYS_CAP_QOS_POLICER_SVC_PROFILE_NUM) - 1;
    spool.user_data_size = sizeof(sys_qos_policer_svc_profile_t);
    spool.desc = "opf-svc-profile";
    spool.key_size = CTC_OFFSET_OF(sys_qos_policer_svc_profile_t, calc_key_len);
    spool.min_index = 1;
    spool.max_index = MCHIP_CAP(SYS_CAP_QOS_POLICER_SVC_PROFILE_NUM) - 1;
    p_usw_qos_policer_master[lchip]->p_svc_profile_pool = ctc_spool_create(&spool);
    if (NULL == p_usw_qos_policer_master[lchip]->p_svc_profile_pool)
    {
        ret =  CTC_E_NO_MEMORY;
    }

    return ret;
}

int32
sys_tmm_qos_policer_spool_deinit(uint8 lchip)
{
    ctc_spool_free(p_usw_qos_policer_master[lchip]->p_svc_profile_pool);

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_init_reg(uint8 lchip)
{
    uint32 cmd = 0;
    uint32 entry_num = 0;
    uint32 index = 0;
    uint8 sup_stormctl_vlan = CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_VLAN_STORMCTL);
    uint8 sup_policer_port  = CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_PORT_POLICER);
    uint8 sup_policer_vlan  = CTC_FLAG_ISSET(p_usw_qos_policer_master[lchip]->policer_svc_mode, CTC_QOS_POLICER_SVC_MODE_VLAN_POLICER);
    uint8 port_vlan_policer_en     = sup_policer_port || sup_policer_vlan;
    uint8 port_vlan_policer_filter = sup_policer_port && sup_policer_vlan;
    IpeSvcPolicingCtl_m ipe_svcpolicing_ctl;
    SvcPolicingCtl_m svcpolicing_ctl;
    SvcPolicingScanCtl_m svcpolicing_scan_ctl;
    SvcPolicingStatsCtl_m svcpolicer_stats_ctl;
    DsIpePolicingPolicy_m ipe_policer_action_profile;
    DsSvcPolicingProfile_m svc_profile;
    TsEngineRefUsNsThreshold_m us_ns_thrd;

    sal_memset(&ipe_svcpolicing_ctl, 0, sizeof(IpeSvcPolicingCtl_m));
    sal_memset(&svcpolicing_ctl, 0, sizeof(SvcPolicingCtl_m));
    sal_memset(&svcpolicing_scan_ctl, 0, sizeof(SvcPolicingScanCtl_m));
    sal_memset(&svcpolicer_stats_ctl, 0, sizeof(SvcPolicingStatsCtl_m));
    sal_memset(&ipe_policer_action_profile, 0, sizeof(DsIpePolicingPolicy_m));
    sal_memset(&svc_profile, 0, sizeof(DsSvcPolicingProfile_m));
    sal_memset(&us_ns_thrd, 0, sizeof(TsEngineRefUsNsThreshold_m));

    cmd = DRV_IOR(IpeSvcPolicingCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &ipe_svcpolicing_ctl));
    SetIpeSvcPolicingCtl(V, internalHeaderAdjustDisable_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, ipgEn_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, muxLengthAdjustDisable_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, maxLengthCheckEn_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, minLengthCheckEn_f, &ipe_svcpolicing_ctl, SYS_QOS_POLICER_SVC_SUPPORT_STORM_CTL(lchip) ? 0 : 1);
    SetIpeSvcPolicingCtl(V, packetLengthMin_f, &ipe_svcpolicing_ctl, 64);
    SetIpeSvcPolicingCtl(V, portVlanPolicerEn_f, &ipe_svcpolicing_ctl, port_vlan_policer_en);
    SetIpeSvcPolicingCtl(V, portVlanPolicerPri_f, &ipe_svcpolicing_ctl, 1);
    SetIpeSvcPolicingCtl(V, portVlanPolicerFilter_f, &ipe_svcpolicing_ctl, port_vlan_policer_filter);
    SetIpeSvcPolicingCtl(V, portVlanPolicerHigh_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, portVlanPolicerSel_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, legacyStormCtlControlMode_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, oamObeyStormCtl_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, ptpObeyStormCtl_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, stormCtlEn_f, &ipe_svcpolicing_ctl, SYS_QOS_POLICER_SVC_SUPPORT_STORM_CTL(lchip) ? 1 : 0);
    SetIpeSvcPolicingCtl(V, portStormCtlEnKeep_f, &ipe_svcpolicing_ctl, 1);
    SetIpeSvcPolicingCtl(V, portStormCtlEn_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, vlanStormCtlEnKeep_f, &ipe_svcpolicing_ctl, 1);
    SetIpeSvcPolicingCtl(V, vlanStormCtlPtrMode_f, &ipe_svcpolicing_ctl, (sup_stormctl_vlan) ? 1 : 0);
    SetIpeSvcPolicingCtl(V, vlanStormCtlEn_f, &ipe_svcpolicing_ctl, (sup_stormctl_vlan) ? 1 : 0);
    SetIpeSvcPolicingCtl(V, svcPolicingAfterAcl_f, &ipe_svcpolicing_ctl, 0);
    SetIpeSvcPolicingCtl(V, svcPolicingColorRwEn_f, &ipe_svcpolicing_ctl, 1);
    SetIpeSvcPolicingCtl(V, svcPolicingDropEn_f, &ipe_svcpolicing_ctl, 1);
    SetIpeSvcPolicingCtl(V, svcPolicingUsePtr0_f, &ipe_svcpolicing_ctl, 1);
    cmd = DRV_IOW(IpeSvcPolicingCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &ipe_svcpolicing_ctl));

    /*config SvcPolicingCtl*/
    SetSvcPolicingCtl(V, dropColor_f, &svcpolicing_ctl, 1);
    SetSvcPolicingCtl(V, statsIntThrd_f, &svcpolicing_ctl, 0xF0000000);
    SetSvcPolicingCtl(V, timestampSel_f, &svcpolicing_ctl, 0);/*0:us,1:ms*/
    SetSvcPolicingCtl(V, pktLenUnit_f, &svcpolicing_ctl, MCHIP_CAP(SYS_CAP_QOS_POLICER_PPS_PACKET_BYTES));
    SetSvcPolicingCtl(V, statsLenUnit_f, &svcpolicing_ctl, MCHIP_CAP(SYS_CAP_QOS_POLICER_PPS_PACKET_BYTES));
    cmd = DRV_IOW(SvcPolicingCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &svcpolicing_ctl));

    SetTsEngineRefUsNsThreshold(V, usNsThreshold_f, &us_ns_thrd, 125000);
    cmd = DRV_IOW(TsEngineRefUsNsThreshold_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &us_ns_thrd));

    /*config SvcPolicingScanCtl*/
    SetSvcPolicingScanCtl(V, ptrMax_f, &svcpolicing_scan_ctl, MCHIP_CAP(SYS_CAP_QOS_POLICER_SVC_POLICER_NUM) - 1);
    SetSvcPolicingScanCtl(V, ptrMin_f, &svcpolicing_scan_ctl, 1);
    SetSvcPolicingScanCtl(V, refreshDis_f, &svcpolicing_scan_ctl, 0);
    SetSvcPolicingScanCtl(V, tickInterval_f, &svcpolicing_scan_ctl, DRV_FROM_AT(lchip) ? 50 : 500);/*56K * Interval(ns) < 0.054s*/
    cmd = DRV_IOW(SvcPolicingScanCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &svcpolicing_scan_ctl));

    /*config SvcPolicingStatsCtl*/
    SetSvcPolicingStatsCtl(V, clearOnRead_f, &svcpolicer_stats_ctl, 1);
    cmd = DRV_IOW(SvcPolicingStatsCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &svcpolicer_stats_ctl));

    sys_usw_ftm_query_table_entry_num(lchip, DsIpePolicingPolicy_t, &entry_num);
    SetDsIpePolicingPolicy(V, colorGreen_f, &ipe_policer_action_profile, CTC_QOS_COLOR_GREEN);
    SetDsIpePolicingPolicy(V, colorRed_f, &ipe_policer_action_profile, CTC_QOS_COLOR_RED);
    SetDsIpePolicingPolicy(V, colorYellow_f, &ipe_policer_action_profile, CTC_QOS_COLOR_YELLOW);
    for (index = 0; index < entry_num; index++)
    {
        cmd = DRV_IOW(DsIpePolicingPolicy_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &ipe_policer_action_profile));
    }

    SetDsSvcPolicingProfile(V, rateX_f, &svc_profile, MCHIP_CAP(SYS_CAP_QOS_POLICER_SVC_TOKEN_RATE));
    SetDsSvcPolicingProfile(V, rateY_f, &svc_profile, MCHIP_CAP(SYS_CAP_QOS_POLICER_SVC_TOKEN_RATE));
    SetDsSvcPolicingProfile(V, tokenThrdX_f, &svc_profile, MCHIP_CAP(SYS_CAP_QOS_POLICER_SVC_TOKEN_THRD));
    SetDsSvcPolicingProfile(V, tokenThrdY_f, &svc_profile, MCHIP_CAP(SYS_CAP_QOS_POLICER_SVC_TOKEN_THRD));
    cmd = DRV_IOW(DsSvcPolicingProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &svc_profile));

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_copp_init(uint8 lchip)
{
    uint32 cmd         = 0;
    uint32 copp_num     = 0;
    IpeCoPPEngineCtl_m ipe_copp_engine_ctl;
    EpeCoPPEngineCtl_m epe_copp_engine_ctl;
    IpeCoPPActionProfile_m action_profile;
    IpeCoPPCtl_m ipe_copp_ctl;
    EpeCoPPCtl_m epe_copp_ctl;

    sal_memset(&ipe_copp_engine_ctl, 0, sizeof(IpeCoPPEngineCtl_m));
    sal_memset(&epe_copp_engine_ctl, 0, sizeof(EpeCoPPEngineCtl_m));
    sal_memset(&action_profile, 0, sizeof(IpeCoPPActionProfile_m));
    sal_memset(&ipe_copp_ctl, 0, sizeof(IpeCoPPCtl_m));
    sal_memset(&epe_copp_ctl, 0, sizeof(EpeCoPPCtl_m));

    sys_usw_ftm_query_table_entry_num(lchip, DsIpeCoPPConfig_t, &copp_num); /*TMM:320*/

    SetIpeCoPPEngineCtl(V, bucketCalculationMode_f, &ipe_copp_engine_ctl, 1);
    SetIpeCoPPEngineCtl(V, bucketPassConditionMode_f, &ipe_copp_engine_ctl, 1);
    SetIpeCoPPEngineCtl(V, interval_f, &ipe_copp_engine_ctl, 1);
    SetIpeCoPPEngineCtl(V, maxPtr_f, &ipe_copp_engine_ctl, copp_num - 1);
    SetIpeCoPPEngineCtl(V, refreshEn_f, &ipe_copp_engine_ctl, 1);
    cmd = DRV_IOW(IpeCoPPEngineCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &ipe_copp_engine_ctl));

    sys_usw_ftm_query_table_entry_num(lchip, DsEpeCoPPConfig_t, &copp_num); /*TMM:16*/
    SetEpeCoPPEngineCtl(V, bucketCalculationMode_f, &epe_copp_engine_ctl, 1);
    SetEpeCoPPEngineCtl(V, bucketPassConditionMode_f, &epe_copp_engine_ctl, 1);
    SetEpeCoPPEngineCtl(V, interval_f, &epe_copp_engine_ctl, 1);
    SetEpeCoPPEngineCtl(V, maxPtr_f, &epe_copp_engine_ctl, copp_num - 1);
    SetEpeCoPPEngineCtl(V, refreshEn_f, &epe_copp_engine_ctl, 1);
    cmd = DRV_IOW(EpeCoPPEngineCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &epe_copp_engine_ctl));


    SetIpeCoPPCtl(V, ipgEnCopp_f,&ipe_copp_ctl,0);
    SetIpeCoPPCtl(V, internalHeaderAdjustDisable_f,&ipe_copp_ctl,0);
    SetIpeCoPPCtl(V, muxLenAdjustDisable_f,&ipe_copp_ctl,0);
    cmd = DRV_IOW(IpeCoPPCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &ipe_copp_ctl));

    SetEpeCoPPCtl(V, ipgEnCopp_f,&epe_copp_ctl,0);
    SetEpeCoPPCtl(V, muxLengthAdjustCoppDisable_f,&epe_copp_ctl,0);
    cmd = DRV_IOW(EpeCoPPCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &epe_copp_ctl));

    SetIpeCoPPActionProfile(V, actProfile_0_decX_f, &action_profile, 0);
    SetIpeCoPPActionProfile(V, actProfile_0_decY_f, &action_profile, 0);
    SetIpeCoPPActionProfile(V, actProfile_0_drop_f, &action_profile, 1);
    SetIpeCoPPActionProfile(V, actProfile_1_decX_f, &action_profile, 1);
    SetIpeCoPPActionProfile(V, actProfile_1_decY_f, &action_profile, 1);
    SetIpeCoPPActionProfile(V, actProfile_1_drop_f, &action_profile, 0);
    cmd = DRV_IOW(IpeCoPPActionProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &action_profile));

    cmd = DRV_IOW(EpeCoPPActionProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &action_profile));

    return CTC_E_NONE;
}

int32
sys_tmm_qos_policer_restore_offset(uint8 lchip, void* p_policer)
{
    sys_usw_opf_t opf;
    sys_qos_policer_t* p_sys_policer = (sys_qos_policer_t*)p_policer;
    uint32 offset = p_sys_policer->policer_ptr;

    sal_memset(&opf, 0, sizeof(sys_usw_opf_t));
    opf.pool_type  = p_usw_qos_policer_master[lchip]->opf_type_policer_config;
    CTC_ERROR_RETURN(_sys_tmm_qos_policer_get_opf_index(lchip, p_sys_policer, &opf.pool_index));

    if (p_sys_policer->entry_size && offset != 0 && p_sys_policer->is_installed == 1)
    {
        CTC_ERROR_RETURN(sys_usw_opf_alloc_offset_from_position(lchip, &opf, p_sys_policer->entry_size, offset));
    }

    return CTC_E_NONE;
}

#define __QUEUE__

int32
_sys_tmm_queue_map_key_lookup_io(uint8 lchip, uint32 tbl_id, uint32 *index, void *p_data)
{
    drv_acc_in_t  lookup_info;
    drv_acc_out_t lookup_result;


    sal_memset(&lookup_info, 0, sizeof(lookup_info));
    sal_memset(&lookup_result, 0, sizeof(lookup_result));

    lookup_info.type = DRV_ACC_TYPE_ALLOC;
    lookup_info.op_type = DRV_ACC_OP_BY_KEY;
    lookup_info.data = p_data;
    lookup_info.tbl_id = tbl_id;
    drv_acc_get_hash_module(lchip, lookup_info.tbl_id, &lookup_info.module);
    CTC_ERROR_RETURN(drv_acc_api(lchip, &lookup_info, &lookup_result));
    *index = lookup_result.key_index;

    if (!lookup_result.is_hit && !lookup_result.is_conflict)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO,"tbl_id:%d, index->0x%x\n", tbl_id,  lookup_result.key_index);
    }
    else if (lookup_result.is_conflict)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Hash Conflict \n");
        return CTC_E_HASH_CONFLICT;
    }
    else if (lookup_result.is_hit)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry already exist \n");
        return CTC_E_EXIST;
    }

    return CTC_E_NONE;
}

 int32
_sys_tmm_queue_map_key_write_io(uint8 lchip, uint32 tbl_id, uint32 index, void *p_data)
{
    drv_acc_in_t  lookup_info;
    drv_acc_out_t lookup_result;

    sal_memset(&lookup_info, 0, sizeof(lookup_info));
    sal_memset(&lookup_result, 0, sizeof(lookup_result));

    lookup_info.type = DRV_ACC_TYPE_ADD;
    lookup_info.op_type = DRV_ACC_OP_BY_INDEX;
    lookup_info.data = p_data;
    lookup_info.tbl_id = tbl_id;
    lookup_info.index = index;
    drv_acc_get_hash_module(lchip, lookup_info.tbl_id, &lookup_info.module);
    CTC_ERROR_RETURN(drv_acc_api(lchip, &lookup_info, &lookup_result));

    return CTC_E_NONE;
}

 int32
_sys_tmm_queue_map_key_delete_io(uint8 lchip, uint32 tbl_id, uint32 index, void *p_data)
{
    drv_acc_in_t  lookup_info;
    drv_acc_out_t lookup_result;

    sal_memset(&lookup_info, 0, sizeof(lookup_info));
    sal_memset(&lookup_result, 0, sizeof(lookup_result));

    lookup_info.type = DRV_ACC_TYPE_DEL;
    lookup_info.op_type = DRV_ACC_OP_BY_INDEX;
    lookup_info.data = p_data;
    lookup_info.tbl_id = tbl_id;
    lookup_info.index = index;
    drv_acc_get_hash_module(lchip, lookup_info.tbl_id, &lookup_info.module);
    CTC_ERROR_RETURN(drv_acc_api(lchip, &lookup_info, &lookup_result));

    return CTC_E_NONE;
}

int32
_sys_tmm_queue_set_defult_profile(uint8 lchip, uint32 group_id)
{
    uint32 index = 0;
    uint32 cmd = 0;
    uint16 queue_num = 0;
    uint32 queue_base = 0;
    DsErmQueueGuaranteedThrdProfile_m guaranteed_profile;
    DsErmQueueLimitedThrdProfile_m ds_que_thrd_profile;
    sal_memset(&guaranteed_profile, 0, sizeof(guaranteed_profile));
    sal_memset(&ds_que_thrd_profile, 0, sizeof(ds_que_thrd_profile));

    if (group_id < MCHIP_CAP(SYS_CAP_QOS_QUEUE_GRP_BASE_16Q))
    {
        queue_base = group_id * MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP);
    }
    else
    {
        queue_base = MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_16Q) + (group_id - MCHIP_CAP(SYS_CAP_QOS_QUEUE_GRP_BASE_16Q)) * MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP_16Q);
    }

    queue_num = (group_id < MCHIP_CAP(SYS_CAP_QOS_QUEUE_GRP_BASE_16Q)) ? MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP) : MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP_16Q);
    SetDsErmQueueGuaranteedThrdProfile(V, queueGuaranteedThrd_f, &guaranteed_profile, 0x3FF);
    /*default disable erm ecn mark*/
    SetDsErmQueueLimitedThrdProfile(V, g_0_ecnMarkThrd_f , &ds_que_thrd_profile, MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD) / 8);
    SetDsErmQueueLimitedThrdProfile(V, g_1_ecnMarkThrd_f , &ds_que_thrd_profile, MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD) / 8);
    SetDsErmQueueLimitedThrdProfile(V, g_2_ecnMarkThrd_f , &ds_que_thrd_profile, MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD) / 8);
    SetDsErmQueueLimitedThrdProfile(V, g_3_ecnMarkThrd_f , &ds_que_thrd_profile, MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD) / 8);
    SetDsErmQueueLimitedThrdProfile(V, g_0_queueLimitedAutoEn_f , &ds_que_thrd_profile, 1);
    SetDsErmQueueLimitedThrdProfile(V, g_1_queueLimitedAutoEn_f , &ds_que_thrd_profile, 1);
    SetDsErmQueueLimitedThrdProfile(V, g_2_queueLimitedAutoEn_f , &ds_que_thrd_profile, 1);
    SetDsErmQueueLimitedThrdProfile(V, g_3_queueLimitedAutoEn_f , &ds_que_thrd_profile, 1);
    for (index = queue_base; index <  queue_base + queue_num; index++)
    {
        cmd = DRV_IOW(DsErmQueueGuaranteedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &guaranteed_profile));
        cmd = DRV_IOW(DsErmQueueLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &ds_que_thrd_profile));
    }
    return CTC_E_NONE;
}

int32
sys_tmm_queue_write_hash_key(uint8 lchip, uint32 service, uint16 dest_port_or_chan,
                                                  uint16 group_id, uint8 type, uint8 is_lsp, int32* p_key_index)
{
    uint32 key_index = 0;
    int32  ret = 0;
    uint32 chan_id = 0;
    uint8  service_type_en = 0;
    uint8  service_type = 0;
    uint8  quemap_type_en = 0;
    uint8  quemap_type = 0;
    uint8  que_offset_mode = 0;
    uint16 queue_base = 0;
    sys_usw_dmps_port_info_t dmps_port_info = {0};
    DsQueueMapHashKey_m QueueMapHash;

    sal_memset(&QueueMapHash, 0, sizeof(DsQueueMapHashKey_m));
    if (SYS_EXTEND_QUE_TYPE_BPE == type || SYS_EXTEND_QUE_TYPE_C2C == type)
    {
        chan_id = dest_port_or_chan;
    }
    else
    {
        dmps_port_info.gport = dest_port_or_chan;
        CTC_ERROR_RETURN(sys_usw_port_api_get_dmps_property(lchip, &dmps_port_info, SYS_PORT_API_DMPS_PROP_CHAN_ID, &chan_id));
        if (!SYS_IS_NETWORK_CHANNEL(chan_id))
        {
            SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Invalid localphy port \n");
            return CTC_E_INVALID_PORT;
        }
    }

    if (SYS_EXTEND_QUE_TYPE_SERVICE == type)
    {
        service_type_en = 1;
        service_type = 0;
    }
    else if (SYS_EXTEND_QUE_TYPE_BPE == type)
    {
        service_type_en = 1;
        service_type = 3;
    }
    else if (SYS_EXTEND_QUE_TYPE_LOGIC_PORT == type)
    {
        service_type_en = 1;
        service_type = is_lsp ? 1 : 2;
    }
    else if (SYS_EXTEND_QUE_TYPE_C2C == type)
    {
        quemap_type_en = 1;
        quemap_type = SYS_QUE_MAP_TYPE_C2C;
        if(p_usw_queue_master[lchip]->queue_num_per_stk_port == 1)
        {
            service_type_en = 1;
            service_type = 3;
            service = 0x40000;
            que_offset_mode = 2;
        }
    }

    if (p_usw_queue_master[lchip]->queue_num_per_stk_port == 1 && SYS_EXTEND_QUE_TYPE_C2C == type)
    {
        queue_base = MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_NETWORK_MISC) + chan_id * MCHIP_CAP(SYS_CAP_QOS_CTL_QUEUE_NUM) + SYS_QUE_MAP_OFFSET_CTL_QUE_CPU;
    }
    else  if (group_id < MCHIP_CAP(SYS_CAP_QOS_QUEUE_GRP_BASE_16Q))
    {
        queue_base = group_id * MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP);
    }
    else
    {
        queue_base = MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_16Q) + (group_id - MCHIP_CAP(SYS_CAP_QOS_QUEUE_GRP_BASE_16Q)) * MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP_16Q);
    }

    SetDsQueueMapHashKey(V, channelId_f,       &QueueMapHash, SYS_USW_CHANNEL_ENCODE(chan_id));
    SetDsQueueMapHashKey(V, fromFabricEn_f,    &QueueMapHash, 0);
    SetDsQueueMapHashKey(V, fromFabric_f,      &QueueMapHash, 0);
    SetDsQueueMapHashKey(V, queMapTypeEn_f,    &QueueMapHash, quemap_type_en);
    SetDsQueueMapHashKey(V, queMapType_f,      &QueueMapHash, quemap_type);
    SetDsQueueMapHashKey(V, serviceEn_f,       &QueueMapHash, service_type_en);
    SetDsQueueMapHashKey(V, serviceType_f,     &QueueMapHash, service_type);
    SetDsQueueMapHashKey(V, service_f,         &QueueMapHash, service);
    SetDsQueueMapHashKey(V, valid_f,           &QueueMapHash, 1);
    SetDsQueueMapHashKey(V, queueBase_f,       &QueueMapHash, queue_base);
    SetDsQueueMapHashKey(V, queueOffsetMode_f, &QueueMapHash, que_offset_mode);

    if(!(SYS_EXTEND_QUE_TYPE_C2C == type && p_usw_queue_master[lchip]->queue_num_per_stk_port == 1))
    {
        CTC_ERROR_RETURN(_sys_tmm_queue_set_defult_profile(lchip, group_id));
    }
    CTC_ERROR_RETURN(_sys_tmm_queue_map_key_lookup_io(lchip, DsQueueMapHashKey_t,
                                                            &key_index, &QueueMapHash));

    CTC_ERROR_RETURN(_sys_tmm_queue_map_key_write_io(lchip, DsQueueMapHashKey_t,
                                                            key_index, &QueueMapHash));

    *p_key_index = key_index;

    return ret;
}

int32
sys_tmm_queue_del_hash_key(uint8 lchip, uint32 key_index)
{
    int32  ret = 0;
    DsQueueMapHashKey_m QueueMapHash;
    sal_memset(&QueueMapHash, 0, sizeof(DsQueueMapHashKey_m));

    SetDsQueueMapHashKey(V, valid_f, &QueueMapHash, 0);
    CTC_ERROR_RETURN(_sys_tmm_queue_map_key_delete_io(lchip, DsQueueMapHashKey_t,
                                                            key_index, &QueueMapHash));
    return ret;
}


int32
_sys_tmm_sch_bind_que_grp_to_chan(uint8 lchip, uint32 group_id, uint16 channel)
{
    uint32 cmd = 0;
    uint32 index = 0;
    uint32 field_val = 0;

    DsQMgrGrpParentSelect_m  que_grp_parent;
    sal_memset(&que_grp_parent, 0, sizeof(DsQMgrGrpParentSelect_m));

    cmd = DRV_IOR(DsQMgrGrpParentSelect_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, group_id, cmd, &que_grp_parent));
    SetDsQMgrGrpParentSelect(V, parentId_f, &que_grp_parent, channel);
    SetDsQMgrGrpParentSelect(V, linkToPortEn_f, &que_grp_parent, 1);
    cmd = DRV_IOW(DsQMgrGrpParentSelect_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, group_id, cmd, &que_grp_parent));

    /*config group high prio, default setting L0 high prio(0: RR 1: L4 high prio 2: L0 high prio)*/
    index = channel;
    field_val = 2;
    cmd = DRV_IOW(DsQMgrDeqChanSchCfg_t, DsQMgrDeqChanSchCfg_schMode_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, index, cmd, &field_val));

    index = channel;
    field_val = group_id;
    cmd = DRV_IOW(DsQMgrPortConfig_t, DsQMgrPortConfig_groupSchId_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, index, cmd, &field_val));

    index = group_id;
    field_val = channel;
    cmd = DRV_IOW(DsQMgrGrpFlowCtlMap_t, DsQMgrGrpFlowCtlMap_channelId_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, index, cmd, &field_val));
    return CTC_E_NONE;
}
int32
_sys_tmm_queue_get_channel_by_group_id(uint8 lchip, uint8  level, uint32 child_sub_grp_id, uint16 *channel)
{
    uint32 cmd = 0;
    uint8  link_to_port = 0;
    uint8  lvl = 0;
    uint32 tbl_index = 0;
    uint32 parent_sub_grp_id = 0;
    uint32 grp_vec_index = 0;
    sys_sch_group_node_t *p_sch_group_node = NULL;
    DsQMgrL0ParentSelect_m parent_select;
    DsQMgrGrpParentSelect_m  que_grp_parent;
    uint32 table_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0ParentSelect_t, DsQMgrL1ParentSelect_t, DsQMgrL2ParentSelect_t,
                                                  DsQMgrL3ParentSelect_t, DsQMgrGrpParentSelect_t};

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&que_grp_parent, 0, sizeof(DsQMgrGrpParentSelect_m));
    sal_memset(&parent_select, 0, sizeof(DsQMgrL0ParentSelect_m));

    if(level == CTC_QOS_SCHED_GROUP_LVL_Q)
    {
        cmd = DRV_IOR(DsQMgrGrpParentSelect_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, child_sub_grp_id, cmd, &que_grp_parent));
        link_to_port = GetDsQMgrGrpParentSelect(V, linkToPortEn_f, &que_grp_parent);
        if (link_to_port)
        {
            *channel = GetDsQMgrGrpParentSelect(V, parentId_f, &que_grp_parent);
            return CTC_E_NONE;
        }
    }

    for (lvl = level + 1; lvl > CTC_QOS_SCHED_GROUP_LVL_R; lvl--)
    {
        uint16 grp_num = 0;
        SYS_QOS_SCH_LVL_GRP_NUM(lvl - 1, grp_num);
        if (0 == grp_num)
        {
            continue;
        }
        SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, lvl - 1, child_sub_grp_id);
        p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
        if (NULL == p_sch_group_node)
        {
            SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
            return CTC_E_NOT_EXIST;
        }

        tbl_index = child_sub_grp_id;
        cmd = DRV_IOR(table_id[lvl - 1], DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select));
        parent_sub_grp_id = GetDsQMgrGrpParentSelect(V, parentId_f, &parent_select);
        child_sub_grp_id = parent_sub_grp_id;
    }

    *channel = parent_sub_grp_id;

    return CTC_E_NONE;
}

int32
sys_tmm_queue_get_channel_by_group_id(uint8 lchip, uint32 group_id, uint16 *channel)
{
    uint32 sub_grp_id = 0;
    uint8  level = 0;

    CTC_PTR_VALID_CHECK(channel);
    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    CTC_ERROR_RETURN(_sys_tmm_queue_get_channel_by_group_id(lchip, level, sub_grp_id, channel));

    return CTC_E_NONE;
}

int32
sys_tmm_queue_get_channel_by_queue_id(uint8 lchip, uint16 queue_id, uint16 *channel)
{
    uint32 sub_grp_id = 0;

    CTC_PTR_VALID_CHECK(channel);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    if(queue_id >= MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_NETWORK_MISC))
    {
        *channel = (queue_id - MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_NETWORK_MISC)) / MCHIP_CAP(SYS_CAP_QOS_CTL_QUEUE_NUM);
    }
    else
    {
        SYS_TMM_GROUP_ID(sub_grp_id, queue_id, 1);
        CTC_ERROR_RETURN(_sys_tmm_queue_get_channel_by_group_id(lchip, CTC_QOS_SCHED_GROUP_LVL_Q, sub_grp_id, channel));
    }
    return CTC_E_NONE;
}


int32
sys_tmm_queue_get_queue_base(uint8 lchip, uint8 channel, uint16 queue_offset, uint16* queue_base)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint16 que_base = 0;
    uint8 step = 0;

    cmd = DRV_IOR(DsErmChannel_t, DsErmChannel_queueBase_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, channel, cmd, &field_val));
    que_base = field_val;
    if(p_usw_queue_master[lchip]->group_id_ets[channel].group_id != INVALID_QUEUE_GROUP_ID_ETS)
    {
        DsErmPrioScTcMap_m sc_tc_map;
        sal_memset(&sc_tc_map, 0, sizeof(DsErmPrioScTcMap_m));
        CTC_MAX_VALUE_CHECK(queue_offset, MCHIP_CAP(SYS_CAP_QOS_NORMAL_QUEUE_NUM));
        cmd = DRV_IOR(DsErmPrioScTcMap_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, queue_offset * 2, cmd, &sc_tc_map));
        step = DsErmPrioScTcMap_g3_1_mappedTcGroup_f - DsErmPrioScTcMap_g3_0_mappedTcGroup_f;
        field_val = GetDsErmPrioScTcMap(V, g3_0_mappedTcGroup_f + step*SYS_ERM_PROFILE_ETS, &sc_tc_map);
        que_base += field_val;
    }
    *queue_base = que_base * MCHIP_CAP(SYS_CAP_QOS_NORMAL_QUEUE_NUM);

    return CTC_E_NONE;
}


int32
_sys_tmm_queue_init_queue_base(uint8 lchip)

{
    int32  ret   = CTC_E_NONE;
    int32  ret_1 = CTC_E_NONE;
    uint8  gchip = 0;
    uint8  group_num = 0;
    uint16 channel = 0;
    uint32 field_val = 0;
    uint32 cmd = 0;
    uint32 group_id = 0;
    uint16 internal_chan_start_dp0 = 0;
    uint16 internal_chan_start_dp1 = 0;

    CTC_ERROR_RETURN(sys_usw_get_gchip_id(lchip, &gchip));
    cmd = DRV_IOW(DsErmChannel_t, DsErmChannel_queueBase_f);

    ret = sys_usw_port_api_get_internal_chan_start(lchip, 0, &internal_chan_start_dp0);
    ret_1 = sys_usw_port_api_get_internal_chan_start(lchip, 1, &internal_chan_start_dp1);

    /*1. network channel queue base init*/
    for (channel = 0; channel < MCHIP_CAP(SYS_CAP_CHANNEL_NUM); channel++)
    {
        if ((channel == MCHIP_CAP(SYS_CAP_CHANID_DMA_RX0))
            ||(CTC_E_NONE == ret   && internal_chan_start_dp0 <= channel && channel < MCHIP_CAP(SYS_CAP_CHANID_CPU_MAC0))
            ||(CTC_E_NONE == ret_1 && internal_chan_start_dp1 <= channel && channel < MCHIP_CAP(SYS_CAP_CHANID_CPU_MAC2)))
        {
            continue;
        }
        CTC_ERROR_RETURN(_sys_usw_queue_alloc_ext_group(lchip, 1, 1, &group_id));
        field_val = group_id;
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, channel, cmd, &field_val));
        _sys_tmm_sch_bind_que_grp_to_chan(lchip, group_id, channel);
    }

    /*2. resrv channel queue base init
    * DROP/LOG/LOOP/OAM/RSV
    */
    /*
    CTC_ERROR_RETURN(_sys_usw_queue_alloc_ext_group(lchip, 1, &group_id));
    field_val = group_id * 8;
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, MCHIP_CAP(SYS_CAP_CHANID_DROP), cmd, &field_val));
    _sys_tsingma_mx_sch_bind_que_grp_to_chan(lchip, group_id, MCHIP_CAP(SYS_CAP_CHANID_DROP));

    CTC_ERROR_RETURN(_sys_usw_queue_alloc_ext_group(lchip, 1, &group_id));
    field_val = group_id * 8;
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, MCHIP_CAP(SYS_CAP_CHANID_ELOG), cmd, &field_val));
    _sys_tsingma_mx_sch_bind_que_grp_to_chan(lchip, group_id, MCHIP_CAP(SYS_CAP_CHANID_ELOG));

    CTC_ERROR_RETURN(_sys_usw_queue_alloc_ext_group(lchip, 1, &group_id));
    field_val = group_id * 8;
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, MCHIP_CAP(SYS_CAP_CHANID_ILOOP), cmd, &field_val));
    _sys_tsingma_mx_sch_bind_que_grp_to_chan(lchip, group_id, MCHIP_CAP(SYS_CAP_CHANID_ILOOP));

    CTC_ERROR_RETURN(_sys_usw_queue_alloc_ext_group(lchip, 1, &group_id));
    field_val = group_id * 8;
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, MCHIP_CAP(SYS_CAP_CHANID_OAM), cmd, &field_val));
    _sys_tsingma_mx_sch_bind_que_grp_to_chan(lchip, group_id, MCHIP_CAP(SYS_CAP_CHANID_OAM));

    CTC_ERROR_RETURN(_sys_usw_queue_alloc_ext_group(lchip, 1, &group_id));
    field_val = group_id * MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP) ;
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, MCHIP_CAP(SYS_CAP_CHANID_RSV), cmd, &field_val));
    _sys_tsingma_mx_sch_bind_que_grp_to_chan(lchip, group_id, MCHIP_CAP(SYS_CAP_CHANID_RSV));
    */

    /*3. dma channel queue base init*/
    group_id = MCHIP_CAP(SYS_CAP_CHANNEL_NUM);
    group_num = p_usw_queue_master[lchip]->queue_num_for_cpu_reason / MCHIP_CAP(SYS_CAP_REASON_GRP_QUEUE_NUM);
    CTC_ERROR_RETURN(_sys_usw_queue_alloc_ext_group_from_position(lchip, group_num, group_id));
    field_val = group_id;
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, MCHIP_CAP(SYS_CAP_CHANID_DMA_RX0), cmd, &field_val));/*tmm only use one channel for dma*/

    return CTC_E_NONE;
}

int32
_sys_tmm_queue_init_queue_map(uint8 lchip)
{
    uint32 cmd = 0;
    uint32 qhash_en = 0;
    uint32 qOffset_mode = 0;
    uint32 qmaptype_en = 0;
    uint32 service_en = 0;
    uint32 service_mask = 0;
    uint32 service_type = 0;
    uint32 service_type_prof0 = 0;
    uint8 index = 0;
    uint8 fromfabric = 0;
    uint8 qmaptype = 0;
    uint8 ermprofid = 0;
    uint8 q_Offset = 0;
    DsQueueMap_m q_map;

    /*ermPorfId=0:for default
     *ermPorfId=1:for user define,see sys_usw_qos_set_resrc_classify_pool,enq same as default
     *ermPorfId=2:for cpu enq,no need queue hash
     *ermPorfId=3:for bpe port ,desmap enq
     *ermPorfId=4:for logic src port enq when service_queue_mode=2 && ingress

     *service_type=0:logic src port enq
     *service_type=1:service id enq
     *service_type=2:logic dest port or nexthopptr enq
     *service_type=3:destmap enq
     */
    if (p_usw_queue_master[lchip]->service_queue_mode == 0)
    {
        service_type_prof0 = SYS_QUE_MAP_ENQ_BY_LOGIC_SRC_PORT;
    }
    else if (p_usw_queue_master[lchip]->service_queue_mode == 1)
    {
        service_type_prof0 = SYS_QUE_MAP_ENQ_BY_SERVICE_ID;
    }
    else if (p_usw_queue_master[lchip]->service_queue_mode == 2)
    {
        service_type_prof0 = SYS_QUE_MAP_ENQ_BY_LOGIC_DST_PORT_OR_NEXTHOPPTR;
    }
    sal_memset(&q_map, 0, sizeof(DsQueueMap_m));
    cmd = DRV_IOW(DsQueueMap_t, DRV_ENTRY_FLAG);
    for (fromfabric = 0; fromfabric < 2; fromfabric++)
    {
        for (qmaptype = 0; qmaptype < SYS_QUE_MAP_TYPE_MAX_NUM; qmaptype++)
        {
            for (ermprofid = 0; ermprofid < SYS_ERM_PROFILE_MAX_NUM; ermprofid++)
            {
                /*init*/
                qhash_en = 1;
                qmaptype_en = 0;
                qOffset_mode = 0;
                service_en = 0;
                service_mask = 0;
                service_type = service_type_prof0;
                q_Offset = 0;

                index = fromfabric << 6 | qmaptype << 3 | ermprofid;
                switch(qmaptype)
                {
                case SYS_QUE_MAP_TYPE_UC:
                    service_en = 1;
                    service_mask = 0x7FFFF;
                    break;
                case SYS_QUE_MAP_TYPE_MC:
                    if (1 == p_usw_queue_master[lchip]->enq_mode)
                    {
                        qhash_en = 0;
                        qOffset_mode = SYS_QUE_MAP_OFFSET_MODE_CTL_QUE;
                        q_Offset = SYS_QUE_MAP_OFFSET_CTL_QUE_MC;
                    }
                    else
                    {
                        service_en = 1;
                        service_mask = 0x7FFFF;
                    }
                    break;
                case SYS_QUE_MAP_TYPE_SPAN:
                    if(2 == p_usw_queue_master[lchip]->enq_mode || 1 == p_usw_queue_master[lchip]->enq_mode)
                    {
                        qhash_en = 0;
                        qOffset_mode = SYS_QUE_MAP_OFFSET_MODE_CTL_QUE;
                        q_Offset = SYS_QUE_MAP_OFFSET_CTL_QUE_SPAN;
                    }
                    else
                    {
                        service_en = 1;
                        service_mask = 0x7FFFF;
                    }
                    break;
                case SYS_QUE_MAP_TYPE_CPU_PKT:
                    qOffset_mode = (ermprofid == SYS_ERM_PROFILE_CPU_PKT) ? SYS_QUE_MAP_OFFSET_MODE_CPU_QUE : 0;
                    if(ermprofid != SYS_ERM_PROFILE_CPU_PKT)
                    {
                        qhash_en = 0;
                        qOffset_mode = SYS_QUE_MAP_OFFSET_MODE_CTL_QUE;
                        q_Offset = SYS_QUE_MAP_OFFSET_CTL_QUE_CPU;
                    }
                    break;
                case SYS_QUE_MAP_TYPE_CPU_PKT_PRIO:
                    qOffset_mode = (ermprofid == SYS_ERM_PROFILE_CPU_PKT) ? SYS_QUE_MAP_OFFSET_MODE_CPU_PRIO_QUE : 0;
                    break;
                case SYS_QUE_MAP_TYPE_C2C:
                    qmaptype_en = 1;
                    if(ermprofid == SYS_ERM_PROFILE_CPU_PKT)
                    {
                        qOffset_mode = SYS_QUE_MAP_OFFSET_MODE_CPU_PRIO_QUE;
                    }
                    break;
                default:
                    break;
                }

                if (p_usw_queue_master[lchip]->queue_num_per_stk_port == 1 && qmaptype == SYS_QUE_MAP_TYPE_C2C && ermprofid != SYS_ERM_PROFILE_CPU_PKT)
                {
                    /*uc*/
                    qOffset_mode = SYS_QUE_MAP_OFFSET_MODE_CTL_QUE;
                    q_Offset = SYS_QUE_MAP_OFFSET_CTL_QUE_C2C;
                    /*mc*/
                    service_en = 1;
                    service_type = SYS_QUE_MAP_ENQ_BY_DESTMAP;
                    service_mask = 0x40000; 
                }
                else if (ermprofid == SYS_ERM_PROFILE_DESTMAP_ENQ)
                {
                    qhash_en = 1;
                    service_en = 1;
                    service_type = SYS_QUE_MAP_ENQ_BY_DESTMAP;
                    service_mask = 0x1FF;
                }
                else if (ermprofid == SYS_ERM_PROFILE_LOGIC_SRC_PORT_ENQ)
                {
                    service_type = SYS_QUE_MAP_ENQ_BY_LOGIC_SRC_PORT;
                }
                else if (ermprofid == SYS_ERM_PROFILE_ETS)
                {
                    qhash_en = 0;
                    qOffset_mode = SYS_QUE_MAP_OFFSET_MODE_ETS_QUE;
                }
                else if (ermprofid == SYS_ERM_PROFILE_NH_GROUP)
                {
                    service_type = SYS_QUE_MAP_ENQ_BY_LOGIC_DST_PORT_OR_NEXTHOPPTR;
                    service_mask = 0x7FFFF;
                }
                else if (ermprofid == SYS_ERM_PROFILE_DEFAULT)
                {
                    service_type = SYS_QUE_MAP_ENQ_BY_SERVICE_ID;
                }
                SetDsQueueMap(V, queMapTypeEn_f, &q_map, qmaptype_en);
                SetDsQueueMap(V, queueHashEn_f, &q_map, qhash_en);
                SetDsQueueMap(V, queueOffsetMode_f, &q_map, qOffset_mode);
                SetDsQueueMap(V, serviceEn_f, &q_map, service_en);
                SetDsQueueMap(V, serviceMask_f, &q_map, service_mask);
                SetDsQueueMap(V, serviceType_f, &q_map, service_type);
                SetDsQueueMap(V, queueOffset_f, &q_map, q_Offset);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &q_map));
            }
        }
    }

    return CTC_E_NONE;
}

int32
sys_tmm_queue_flush(uint8 lchip,uint16 chan_id,uint16 group_id_l4,uint8 flag, uint16 queue_id)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint32 group_id_l0 = 0;
    uint8 is_bind = 0;
    uint8 config_flag = 0;
    uint16 queue_offset = 0;
    uint16 queue_base = 0;
    uint16 queue_num = 0;

    CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, chan_id, &is_bind, &group_id_l0));
    if (!is_bind)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR,  "channel has not bind L0,channel = %d\n", chan_id);
        return CTC_E_NOT_EXIST;
    }

    /*flush queue*/
    CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, group_id_l0, CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_FLUSH_EN),
                                                   CTC_QOS_SCHED_GROUP_LVL_Q, TRUE));
    field_val = 1;
    cmd = DRV_IOW(DsQMgrGrpConfig_t, DsQMgrGrpConfig_flushEn_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_id_l4, cmd, &field_val));

    if (flag)
    {
        queue_base = queue_id;
        queue_num = 1;
    }
    else
    {
        SYS_GET_QUEUE_BASE_BY_GROUP_ID(group_id_l4, queue_base, queue_num);
    }

    for (queue_offset = queue_base; queue_offset < queue_base + queue_num; queue_offset++)
    {
        CTC_ERROR_RETURN(_sys_usw_check_queue_status(lchip, queue_offset));
    }

    CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, group_id_l0, CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_FLUSH_EN),
                                                   CTC_QOS_SCHED_GROUP_LVL_Q, FALSE));
    field_val = 0;
    cmd = DRV_IOW(DsQMgrGrpConfig_t, DsQMgrGrpConfig_flushEn_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_id_l4, cmd, &field_val));
    return CTC_E_NONE;
}

int32
sys_tmm_queue_set_port_drop_en(uint8 lchip, uint16 chan_id, bool enable)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint32 value[8] = {0};
    uint32 group_id = 0;
    uint32 q_group = 0;
    uint8  link_to_port_en = 0;
    uint8  is_bind = 0;
    uint8  config_flag = 0;
    ErmMiscCtl_m erm_misc_ctl;
    DsQMgrGrpConfig_m q_group_cfg; 
    DsErmPortLimitedThrdProfile_m erm_port_limit_prof;
    sal_memset(&erm_port_limit_prof, 0, sizeof(DsErmPortLimitedThrdProfile_m));

    CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, chan_id, &is_bind, &group_id));
    CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_queue_group(lchip, chan_id, &link_to_port_en, &q_group));

    if (enable)
    {
        /*1. Enable Drop incoming channel packets*/
        cmd = DRV_IOR(DsErmPortLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &erm_port_limit_prof));
        SetDsErmPortLimitedThrdProfile(V, g_0_portLimitedThrd_f, &erm_port_limit_prof, 0);
        SetDsErmPortLimitedThrdProfile(V, g_1_portLimitedThrd_f, &erm_port_limit_prof, 0);
        SetDsErmPortLimitedThrdProfile(V, g_2_portLimitedThrd_f, &erm_port_limit_prof, 0);
        SetDsErmPortLimitedThrdProfile(V, g_3_portLimitedThrd_f, &erm_port_limit_prof, 0);
        cmd = DRV_IOW(DsErmPortLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &erm_port_limit_prof));

        field_val = 0;
        cmd = DRV_IOW(DsErmPortGuaranteedThrdProfile_t, DsErmPortGuaranteedThrdProfile_portGuaranteedThrd_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, chan_id, cmd, &field_val));

        /*1.1 disable queue guarantee*/  
        cmd = DRV_IOR(ErmMiscCtl_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &erm_misc_ctl));
        SetErmMiscCtl(V, guaranteedMode_f, &erm_misc_ctl, 0);/*bit0:porttc 1:portsc 2:port guarantee*/
        SetErmMiscCtl(V, thrdPassMode_f, &erm_misc_ctl, 0);
        cmd = DRV_IOW(ErmMiscCtl_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &erm_misc_ctl));

        /*2. enable channel flush*/
        cmd = DRV_IOR(QMgrPortFlushCtl_t, QMgrPortFlushCtl_flushEn_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &value));
        CTC_BIT_SET(value[chan_id / 32], chan_id % 32);
        if (DRV_IS_TMG(lchip))
        {
            /* bit 0-127 is chan; 128-255 is ctl queue grp */
            CTC_BIT_SET(value[(MCHIP_CAP(SYS_CAP_CHANNEL_NUM)+chan_id)/32], (MCHIP_CAP(SYS_CAP_CHANNEL_NUM)+chan_id)%32);
        }
        cmd = DRV_IOW(QMgrPortFlushCtl_t, QMgrPortFlushCtl_flushEn_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &value));

        /*3. enable L0~L4 sch group flush*/
        if(is_bind)
        {
            CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_FLUSH_EN);
            CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, group_id, config_flag, CTC_QOS_SCHED_GROUP_LVL_MAX, TRUE));
        }
        if (link_to_port_en)
        {
            cmd = DRV_IOR(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, q_group, cmd, &q_group_cfg));
            SetDsQMgrGrpConfig(V, flushEn_f, &q_group_cfg, 1);
            cmd = DRV_IOW(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, q_group, cmd, &q_group_cfg));
        }
    }
    else
    {
        /*1. disable L0~L4 sch group flush*/
        if(is_bind)
        {
            CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_FLUSH_EN);
            CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, group_id, config_flag, CTC_QOS_SCHED_GROUP_LVL_MAX, FALSE));
        }
        if (link_to_port_en)
        {
            cmd = DRV_IOR(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, q_group, cmd, &q_group_cfg));
            SetDsQMgrGrpConfig(V, flushEn_f, &q_group_cfg, 0);
            cmd = DRV_IOW(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, q_group, cmd, &q_group_cfg));
        }
        /*2. disbale channel flush*/
        cmd = DRV_IOR(QMgrPortFlushCtl_t, QMgrPortFlushCtl_flushEn_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &value));
        CTC_BIT_UNSET(value[chan_id / 32], chan_id % 32);
        if (DRV_IS_TMG(lchip))
        {
            /* bit 0-127 is chan; 128-255 is ctl queue grp */
            CTC_BIT_UNSET(value[(MCHIP_CAP(SYS_CAP_CHANNEL_NUM)+chan_id)/32], (MCHIP_CAP(SYS_CAP_CHANNEL_NUM)+chan_id)%32);
        }
        cmd = DRV_IOW(QMgrPortFlushCtl_t, QMgrPortFlushCtl_flushEn_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &value));

        /*2.1 disable queue guarantee*/ 
        cmd = DRV_IOR(ErmMiscCtl_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &erm_misc_ctl));
        SetErmMiscCtl(V, guaranteedMode_f, &erm_misc_ctl, 7);/*bit0:porttc 1:portsc 2:port guarantee*/
        SetErmMiscCtl(V, thrdPassMode_f, &erm_misc_ctl, 7);
        cmd = DRV_IOW(ErmMiscCtl_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &erm_misc_ctl));

        /*3. Disable Drop incoming channel packets*/
        cmd = DRV_IOR(DsErmPortLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &erm_port_limit_prof));
        SetDsErmPortLimitedThrdProfile(V, g_0_portLimitedThrd_f, &erm_port_limit_prof, 0x3FFF);
        SetDsErmPortLimitedThrdProfile(V, g_1_portLimitedThrd_f, &erm_port_limit_prof, 0x3FFF);
        SetDsErmPortLimitedThrdProfile(V, g_2_portLimitedThrd_f, &erm_port_limit_prof, 0x3FFF);
        SetDsErmPortLimitedThrdProfile(V, g_3_portLimitedThrd_f, &erm_port_limit_prof, 0x3FFF);
        cmd = DRV_IOW(DsErmPortLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &erm_port_limit_prof));

        field_val = 30;
        cmd = DRV_IOW(DsErmPortGuaranteedThrdProfile_t, DsErmPortGuaranteedThrdProfile_portGuaranteedThrd_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, chan_id, cmd, &field_val));
    }

    return CTC_E_NONE;
}

int32
sys_tmm_qos_queue_init(uint8 lchip)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint8 chan_num = 0;
    uint8 group_num = 0;
    uint8 step = 0;
    uint16 i = 0;
    uint8 j = 0;
    uint8 grp_num_per_ring = 0;
    uint16 group_vec = 0;

    QWriteCtl_m  q_write_ctl;
    EpeHdrEditToDmaCtl_m epe_to_dma_ctl;
    DmaRxStallMap_m dma_rx_map;
    DsQMgrFlowCtlMap0_m fc_map;
    QMgrDmaStallCtl_m dma_stall_ctl;
    EpePktProcQueBaseCtl_m q_base_ctl;
    DsQueueIdBase_m q_base_int;
    sal_memset(&q_write_ctl, 0, sizeof(QWriteCtl_m));
    sal_memset(&epe_to_dma_ctl, 0, sizeof(EpeHdrEditToDmaCtl_m));
    sal_memset(&dma_rx_map, 0, sizeof(DmaRxStallMap_m));
    sal_memset(&fc_map, 0, sizeof(DsQMgrFlowCtlMap0_m));
    sal_memset(&dma_stall_ctl, 0, sizeof(QMgrDmaStallCtl_m));
    sal_memset(&q_base_ctl, 0, sizeof(EpePktProcQueBaseCtl_m));
    sal_memset(&q_base_int, 0, sizeof(DsQueueIdBase_m));

    _sys_tmm_queue_init_queue_base(lchip);

    /*config QWriteCtl*/
    cmd = DRV_IOR(QWriteCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &q_write_ctl));
    SetQWriteCtl(V, useLogicDestPort_f, &q_write_ctl, !p_usw_queue_master[lchip]->service_queue_mode);
    SetQWriteCtl(V, c2cPacketQueMapType_f, &q_write_ctl, (SYS_C2C_QUEUE_MODE(lchip)) ? SYS_QUE_MAP_TYPE_C2C : 0);
    SetQWriteCtl(V, hash_0_keyAddrBase_f, &q_write_ctl, 0);
    SetQWriteCtl(V, hash_1_keyAddrBase_f, &q_write_ctl, 1 << 10);
    SetQWriteCtl(V, hash_2_keyAddrBase_f, &q_write_ctl, 1 << 11);
    SetQWriteCtl(V, hash_3_keyAddrBase_f, &q_write_ctl, 3 << 11);
    cmd = DRV_IOW(QWriteCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &q_write_ctl));

    /*CID[15,8] & sevice id conflict */
    if(p_usw_queue_master[lchip]->service_queue_mode != 1)
    {
        /*Global disable Service ID, CID support 16 bit*/
        field_val = 1;
        cmd = DRV_IOW(IpeLoopbackHeaderAdjustCtl_t, IpeLoopbackHeaderAdjustCtl_supportExtLenCid_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));
        cmd = DRV_IOW(IpeFwdCtl_t, IpeFwdCtl_supportExtLenCid_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));
        cmd = DRV_IOW(EpeHdrAdjustCtl_t, EpeHdrAdjustCtl_supportExtLenCid_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));
        cmd = DRV_IOW(EpeHeaderEditCtl_t, EpeHeaderEditCtl_supportExtLenCid_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));
        cmd = DRV_IOW(EpeHeaderEditCtl_t, EpeHeaderEditCtl_toCpuWithCidExt_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));
        MCHIP_CAP(SYS_CAP_ACL_MAX_CID_VALUE) = 65535;
    }
    else
    {
        /*enbale Service ID and use Service to EnQueue. CID only support 8 bit */
        MCHIP_CAP(SYS_CAP_ACL_MAX_CID_VALUE) = 255;
    }
    _sys_tmm_queue_init_queue_map(lchip);

    /*config queue group select dma ring*/
    step = EpeHdrEditToDmaCtl_g_1_dmaChanId_f - EpeHdrEditToDmaCtl_g_0_dmaChanId_f;
    chan_num = p_usw_queue_master[lchip]->max_dma_rx_num; /*mean dma ring*/
    group_num = p_usw_queue_master[lchip]->queue_num_for_cpu_reason / MCHIP_CAP(SYS_CAP_REASON_GRP_QUEUE_NUM);
    grp_num_per_ring = group_num / chan_num;
    for (i = 0; i < group_num; i++)
    {
        SetEpeHdrEditToDmaCtl(V, g_0_dmaChanId_f + step * i, &epe_to_dma_ctl, i / grp_num_per_ring);
    }
    SetEpeHdrEditToDmaCtl(V, lxGrpBaseMin_f, &epe_to_dma_ctl, MCHIP_CAP(SYS_CAP_CHANNEL_NUM));
    SetEpeHdrEditToDmaCtl(V, lxGrpBaseMax_f, &epe_to_dma_ctl, MCHIP_CAP(SYS_CAP_CHANNEL_NUM) + group_num - 1);
    SetEpeHdrEditToDmaCtl(V, lxSel_f, &epe_to_dma_ctl, 0);
    cmd = DRV_IOW(EpeHdrEditToDmaCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &epe_to_dma_ctl));

    SetQMgrDmaStallCtl(V, queGrpBase0_f, &dma_stall_ctl, MCHIP_CAP(SYS_CAP_CHANNEL_NUM));
    SetQMgrDmaStallCtl(V, queGrpBase1_f, &dma_stall_ctl, MCHIP_CAP(SYS_CAP_CHANNEL_NUM) + group_num - 1);
    cmd = DRV_IOW(QMgrDmaStallCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &dma_stall_ctl));

    /*config dma flow control*/
    step = DmaRxStallMap_g_1_queGrpVec_f - DmaRxStallMap_g_0_queGrpVec_f;
    grp_num_per_ring = group_num / chan_num;
    for (i = 0; i < chan_num; i++)
    {
        group_vec = 0;
        for (j = 0; j < grp_num_per_ring; j++)
        {
            CTC_BIT_SET(group_vec, grp_num_per_ring * i + j);
        }
        SetDmaRxStallMap(V, g_0_queGrpVec_f + step * i, &dma_rx_map, group_vec);
    }
    cmd = DRV_IOW(DmaRxStallMap_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &dma_rx_map));

    return CTC_E_NONE;
}

#define __SHAPE__

int32
_sys_tmm_shape_init_queue_shp(uint8 lchip)
{
    DsQMgrQueShpProfile0_m queue_profile0;
    DsQMgrCtlQueShpProfile_m ctl_queue_profile;
    uint32 cmd = 0;
    uint32 cmd1 = 0;
    uint32 max_size = 0;
    uint16 index = 0;
    uint16 step = 0;

    sal_memset(&queue_profile0, 0, sizeof(DsQMgrQueShpProfile0_m));
    sal_memset(&ctl_queue_profile, 0, sizeof(DsQMgrCtlQueShpProfile_m));

    /* default disable queue shape*/
    sys_usw_ftm_query_table_entry_num(lchip, DsQMgrQueShpProfile0_t, &max_size);
    step = DsQMgrQueShpProfile0_g_1_shpProfile_tokenGran_f - DsQMgrQueShpProfile0_g_0_shpProfile_tokenGran_f;

    for (index = 0; index < MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_GRP); index++)
    {
        SetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenGran_f + index * step, &queue_profile0, 7);
        SetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenRate_f + index * step, &queue_profile0, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE));
        SetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenThrd_f + index * step, &queue_profile0, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD));
    }
    cmd = DRV_IOW(DsQMgrQueShpProfile0_t, DRV_ENTRY_FLAG);
    cmd1 = DRV_IOW(DsQMgrQueShpProfile1_t, DRV_ENTRY_FLAG);
    for (index = 0; index < max_size; index++)
    {
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &queue_profile0));
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd1, &queue_profile0));
    }

    /* default disable ctl queue shape*/
    sys_usw_ftm_query_table_entry_num(lchip, DsQMgrCtlQueShpProfile_t, &max_size);
    step = DsQMgrCtlQueShpProfile_g_1_shpProfile_tokenGran_f - DsQMgrCtlQueShpProfile_g_0_shpProfile_tokenGran_f;
    for (index = 0; index < MCHIP_CAP(SYS_CAP_QOS_CTL_QUEUE_NUM); index++)
    {
        SetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenGran_f + index * step, &ctl_queue_profile, 7);
        SetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenRate_f + index * step, &ctl_queue_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE));
        SetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenThrd_f + index * step, &ctl_queue_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD));
    }
    cmd = DRV_IOW(DsQMgrCtlQueShpProfile_t, DRV_ENTRY_FLAG);
    for (index = 0; index < max_size; index++)
    {
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &ctl_queue_profile));
    }

    return CTC_E_NONE;
}

int32
_sys_tmm_shape_init_group_shp(uint8 lchip)
{
    uint32 cmd = 0;
    uint16 index = 0;
    uint8  step = 0;
    uint8  grp_lvl = 0;
    DsQMgrGrpShpProfile_m grp_shp_profile;
    DsQMgrL0NodeShpProfile_m grp_node_shp_profile;
    DsQMgrGrpNodeShpProfile_m que_grp_node_shp_profile;


    uint32 grp_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0ShpProfile_t, DsQMgrL1ShpProfile_t,
                                                    DsQMgrL2ShpProfile_t, DsQMgrL3ShpProfile_t,
                                                    DsQMgrGrpShpProfile_t};

    uint32 node_pir_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpProfile_t, DsQMgrL1NodeShpProfile_t,
                                                         DsQMgrL2NodeShpProfile_t, DsQMgrL3NodeShpProfile_t,
                                                         DsQMgrGrpNodeShpProfile_t};

    uint32 lvl_grp_num[CTC_QOS_SCHED_GROUP_LVL_MAX] = {MCHIP_CAP(SYS_CAP_QOS_L0_GRP_NUM), MCHIP_CAP(SYS_CAP_QOS_L1_GRP_NUM),
                                                     MCHIP_CAP(SYS_CAP_QOS_L2_GRP_NUM), MCHIP_CAP(SYS_CAP_QOS_L3_GRP_NUM),
                                                     MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NUM)};


    sal_memset(&grp_shp_profile, 0, sizeof(DsQMgrGrpShpProfile_m));
    sal_memset(&grp_node_shp_profile, 0, sizeof(DsQMgrL0NodeShpProfile_m));
    sal_memset(&que_grp_node_shp_profile, 0, sizeof(DsQMgrGrpNodeShpProfile_m));

    /* default disable grp shape*/
    SetDsQMgrGrpShpProfile(V, tokenRate_f, &grp_shp_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE));
    SetDsQMgrGrpShpProfile(V, tokenThrd_f, &grp_shp_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD));
    SetDsQMgrGrpShpProfile(V, tokenGran_f, &grp_shp_profile, 7);

    for (grp_lvl = 0; grp_lvl < CTC_QOS_SCHED_GROUP_LVL_MAX; grp_lvl++)
    {
        cmd = DRV_IOW(grp_tbl_id[grp_lvl], DRV_ENTRY_FLAG);
        for (index = 0; index < lvl_grp_num[grp_lvl]; index++)
        {
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &grp_shp_profile));
        }
    }

    /* default disable que grp node shape*/
    step = DsQMgrGrpNodeShpProfile_g_1_shpProfile0_ppsMode_f - DsQMgrGrpNodeShpProfile_g_0_shpProfile0_ppsMode_f;
    for (index = 0; index < MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NODE_NUM); index++)
    {
        SetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile_tokenRate_f + index * step, &que_grp_node_shp_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE));
        SetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile_tokenThrd_f + index * step, &que_grp_node_shp_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD));
        SetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile_tokenGran_f + index * step, &que_grp_node_shp_profile, 7);
    }
    cmd = DRV_IOW(DsQMgrGrpNodeShpProfile_t, DRV_ENTRY_FLAG);
    for (index = 0; index < MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NUM); index++)
    {
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &que_grp_node_shp_profile));
    }

    /* default disable Lx node shape*/
    step = DsQMgrL0NodeShpProfile0_g_1_shpProfile_ppsMode_f - DsQMgrL0NodeShpProfile0_g_0_shpProfile_ppsMode_f;
    for (index = 0; index < MCHIP_CAP(SYS_CAP_QOS_LX_GRP_NODE_NUM) ; index++)
    {
        SetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenRate_f + index * step, &grp_node_shp_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE));
        SetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenThrd_f + index * step, &grp_node_shp_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD));
        SetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenGran_f + index * step, &grp_node_shp_profile, 7);
    }

    for (grp_lvl = 0; grp_lvl < CTC_QOS_SCHED_GROUP_LVL_Q; grp_lvl++)
    {
        cmd = DRV_IOW(node_pir_tbl_id[grp_lvl], DRV_ENTRY_FLAG);
        for (index = 0; index < lvl_grp_num[grp_lvl]; index++)
        {
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &grp_node_shp_profile));
        }
    }

    return CTC_E_NONE;
}

int32
_sys_tmm_shape_init_chan_shp(uint8 lchip)
{
    uint16 channel_id              = 0;
    uint32 cmd                     = 0;
    DsQMgrPortShpProfile_m chan_shp_profile;
    ErmAqmPortScanCtl_m aqm_port_ctl;

    sal_memset(&chan_shp_profile, 0, sizeof(DsQMgrPortShpProfile_m));
    sal_memset(&aqm_port_ctl, 0, sizeof(ErmAqmPortScanCtl_m));

    /* default disable chan shape*/
    SetDsQMgrPortShpProfile(V, tokenRate_f, &chan_shp_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE));
    SetDsQMgrPortShpProfile(V, tokenThrd_f, &chan_shp_profile, MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD));
    SetDsQMgrPortShpProfile(V, tokenGran_f, &chan_shp_profile, 7);

    for (channel_id = 0; channel_id < MCHIP_CAP(SYS_CAP_CHANNEL_NUM); channel_id++)
    {
        cmd = DRV_IOW(DsQMgrPortShpProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, channel_id, cmd, &chan_shp_profile));
    }

    SetErmAqmPortScanCtl(V, aqmPortScanEn_f, &aqm_port_ctl, 1);
    SetErmAqmPortScanCtl(V, aqmPortScanInterval_f, &aqm_port_ctl, 150);
    SetErmAqmPortScanCtl(V, aqmPortScanPtrMax_f, &aqm_port_ctl, (MCHIP_CAP(SYS_CAP_CHANNEL_NUM) - 1));
    SetErmAqmPortScanCtl(V, aqmPortScanPtrPhyMax_f, &aqm_port_ctl, MCHIP_CAP(SYS_CAP_CHANNEL_NUM));
    cmd = DRV_IOW(ErmAqmPortScanCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &aqm_port_ctl));

    return CTC_E_NONE;
}

int32
sys_tmm_qos_set_reason_shp_base_pkt_en(uint8 lchip, uint8 enable)
{
    uint8  index = 0;
    uint8  index1 = 0;
    uint8  step = 0;
    uint8  grp_num = 0;
    uint16 grp_id = 0;
    uint32 grp_base = 0;
    uint32 group_index = 0;
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint32 table_id_grp[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0ShpProfile_t, DsQMgrL1ShpProfile_t, DsQMgrL2ShpProfile_t,
                                                        DsQMgrL3ShpProfile_t, DsQMgrGrpShpProfile_t};
    uint32 field_id_grp[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0ShpProfile_ppsMode_f, DsQMgrL1ShpProfile_ppsMode_f,
                                                        DsQMgrL2ShpProfile_ppsMode_f, DsQMgrL3ShpProfile_ppsMode_f,
                                                        DsQMgrGrpShpProfile_ppsMode_f};
    uint32 table_id_grp_node[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpProfile_t, DsQMgrL1NodeShpProfile_t,
                                                             DsQMgrL2NodeShpProfile_t, DsQMgrL3NodeShpProfile_t,
                                                             DsQMgrGrpNodeShpProfile_t};
    uint32 field_id_grp_node[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpProfile_g_0_shpProfile_ppsMode_f,
                                                             DsQMgrL1NodeShpProfile_g_0_shpProfile_ppsMode_f,
                                                             DsQMgrL2NodeShpProfile_g_0_shpProfile_ppsMode_f,
                                                             DsQMgrL3NodeShpProfile_g_0_shpProfile_ppsMode_f,
                                                             DsQMgrGrpNodeShpProfile_g_0_shpProfile_ppsMode_f};

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    field_val = enable ? 1 : 0;
    if (MCHIP_DMPS(lchip)->cpumac_get_lport_info_by_inner_idx)
    {
        uint8  is_network = 0;
        uint16 lport = 0;
        uint16 cpu_mac_channel[SYS_USW_CPU_MAC_NUM] = {MCHIP_CAP(SYS_CAP_CHANID_CPU_MAC0), MCHIP_CAP(SYS_CAP_CHANID_CPU_MAC1),
                                                                  MCHIP_CAP(SYS_CAP_CHANID_CPU_MAC2), MCHIP_CAP(SYS_CAP_CHANID_CPU_MAC3)};

        for (index = 0; index < SYS_USW_CPU_MAC_NUM; index++)
        {
             MCHIP_DMPS(lchip)->cpumac_get_lport_info_by_inner_idx(lchip,index,&lport,&is_network);
             if(!is_network)
             {
                 cmd = DRV_IOW(DsQMgrPortShpProfile_t, DsQMgrPortShpProfile_ppsMode_f);
                 CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, cpu_mac_channel[index], cmd, &field_val));
             }
        }

    }

    /*set chan profile pps*/
    cmd = DRV_IOW(DsQMgrPortShpProfile_t, DsQMgrPortShpProfile_ppsMode_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, MCHIP_CAP(SYS_CAP_CHANID_DMA_RX0), cmd, &field_val));

    /*set L0/L1/L2/L3 group and group node profile pps*/
    step = DsQMgrL0NodeShpProfile_g_1_shpProfile_ppsMode_f - DsQMgrL0NodeShpProfile_g_0_shpProfile_ppsMode_f;
    for (index = CTC_QOS_SCHED_GROUP_LVL_R; index < CTC_QOS_SCHED_GROUP_LVL_Q; index++)
    {
        cmd = DRV_IOW(table_id_grp[index], field_id_grp[index]);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));

        for (index1 = 0; index1 < MCHIP_CAP(SYS_CAP_QOS_LX_GRP_NODE_NUM); index1++)
        {
            cmd = DRV_IOW(table_id_grp_node[index], field_id_grp_node[index] + index1 * step);
            CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));
        }
    }
    /*set L4 group and group node profile pps*/
    grp_num = p_usw_queue_master[lchip]->queue_num_for_cpu_reason / MCHIP_CAP(SYS_CAP_REASON_GRP_QUEUE_NUM);
    grp_base = MCHIP_CAP(SYS_CAP_CHANNEL_NUM);
    step = DsQMgrGrpNodeShpProfile_g_1_shpProfile0_ppsMode_f - DsQMgrGrpNodeShpProfile_g_0_shpProfile0_ppsMode_f;
    for (index = 0; index < grp_num; index++)
    {
        cmd = DRV_IOW(table_id_grp[CTC_QOS_SCHED_GROUP_LVL_Q], field_id_grp[CTC_QOS_SCHED_GROUP_LVL_Q]);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, grp_base + index, cmd, &field_val));

        for (index1 = 0; index1 < MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NODE_NUM); index1++)
        {
            cmd = DRV_IOW(table_id_grp_node[CTC_QOS_SCHED_GROUP_LVL_Q], field_id_grp_node[CTC_QOS_SCHED_GROUP_LVL_Q] + index1 * step);
            CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, grp_base + index, cmd, &field_val));
        }
    }

    /*set queue profile pps*/
    SYS_TMM_GROUP_ID(grp_id, (MCHIP_CAP(SYS_CAP_CHANNEL_NUM)*MCHIP_CAP(SYS_CAP_QOS_QUEUE_NUM_PER_CHAN)), 1);
    SYS_TMM_GROUP_INDEX(group_index, grp_id);
    step = DsQMgrQueShpProfile0_g_1_shpProfile_ppsMode_f - DsQMgrQueShpProfile0_g_0_shpProfile_ppsMode_f;
    for (index = 0; index < grp_num / 2; index++)
    {
        for (index1 = 0; index1 < MCHIP_CAP(SYS_CAP_REASON_GRP_QUEUE_NUM); index1++)
        {
            cmd = DRV_IOW(DsQMgrQueShpProfile0_t, DsQMgrQueShpProfile0_g_0_shpProfile_ppsMode_f + index1 * step);
            CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_index + index, cmd, &field_val));

            cmd = DRV_IOW(DsQMgrQueShpProfile1_t, DsQMgrQueShpProfile1_g_0_shpProfile_ppsMode_f + index1 * step);
            CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_index + index, cmd, &field_val));
        }
    }

    p_usw_queue_master[lchip]->shp_pps_en = enable;
    SYS_USW_REGISTER_WB_SYNC_EN(lchip, CTC_FEATURE_QOS, SYS_WB_APPID_QOS_SUBID_QUEUE_MASTER, 1);
    return CTC_E_NONE;
}

int32
_sys_tmm_shape_init_shp_ctl(uint8 lchip)
{
    uint32 cmd = 0;
    uint8  grp_lvl = 0;
    QMgrL0ShpCtl_m grp_shp_ctl;
    QMgrPortShpCtl_m port_shp_ctl;
    uint32 lvl_grp_num[CTC_QOS_SCHED_GROUP_LVL_MAX] = {MCHIP_CAP(SYS_CAP_QOS_L0_GRP_NUM), MCHIP_CAP(SYS_CAP_QOS_L1_GRP_NUM),
                                                     MCHIP_CAP(SYS_CAP_QOS_L2_GRP_NUM), MCHIP_CAP(SYS_CAP_QOS_L3_GRP_NUM),
                                                     MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NUM)};
    uint32 shp_ctl[CTC_QOS_SCHED_GROUP_LVL_MAX] = {QMgrL0ShpCtl_t, QMgrL1ShpCtl_t, QMgrL2ShpCtl_t,
                                                 QMgrL3ShpCtl_t, QMgrQueGrpShpCtl_t};

    sal_memset(&grp_shp_ctl, 0, sizeof(QMgrL0ShpCtl_m));
    sal_memset(&port_shp_ctl, 0, sizeof(QMgrPortShpCtl_m));

    /* cfg grp shp scan*/
    SetQMgrL0ShpCtl(V, shpPtrMin_f, &grp_shp_ctl, 0);
    SetQMgrL0ShpCtl(V, shpRefreshDis_f, &grp_shp_ctl, 0);
    SetQMgrL0ShpCtl(V, shpTickInterval_f, &grp_shp_ctl, 1);

    if (DRV_IS_TMG(lchip))
    {
	    QMgrQueGrpShpCtl_m que_shp_ctl;
		sal_memset(&que_shp_ctl, 0, sizeof(QMgrQueGrpShpCtl_m));
		
        SetQMgrL0ShpCtl(V, shpPtrMax_f, &grp_shp_ctl, MCHIP_CAP(SYS_CAP_QOS_L0_GRP_NUM) - 1);
        cmd = DRV_IOW(QMgrL0ShpCtl_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &grp_shp_ctl));

        SetQMgrQueGrpShpCtl(V, shpPtrMin_f, &que_shp_ctl, 0);
        SetQMgrQueGrpShpCtl(V, shpRefreshDis_f, &que_shp_ctl, 0);
        SetQMgrQueGrpShpCtl(V, shpTickInterval_f, &que_shp_ctl, 1);
        SetQMgrQueGrpShpCtl(V, shpPtrMax_f, &que_shp_ctl, MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NUM) - 1);
        SetQMgrQueGrpShpCtl(V, shpPtrMaxExtra_f, &que_shp_ctl, MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NUM) + MCHIP_CAP(SYS_CAP_CHANNEL_NUM) - 1);
        cmd = DRV_IOW(QMgrQueGrpShpCtl_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &que_shp_ctl));
    }
    else
    {
        for (grp_lvl = 0; grp_lvl < CTC_QOS_SCHED_GROUP_LVL_MAX; grp_lvl++)
        {
            SetQMgrL0ShpCtl(V, shpPtrMax_f, &grp_shp_ctl, lvl_grp_num[grp_lvl]-1);
            cmd = DRV_IOW(shp_ctl[grp_lvl], DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &grp_shp_ctl));
        }
    }

    /* cfg port shp scan*/
    SetQMgrPortShpCtl(V, shpPtrMax_f, &port_shp_ctl, (MCHIP_CAP(SYS_CAP_CHANNEL_NUM) - 1));
    SetQMgrPortShpCtl(V, shpPtrMin_f, &port_shp_ctl, 0);
    SetQMgrPortShpCtl(V, shpRefreshDis_f, &port_shp_ctl, 0);
    SetQMgrPortShpCtl(V, shpTickInterval_f, &port_shp_ctl, 1);
    cmd = DRV_IOW(QMgrPortShpCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &port_shp_ctl));

    return CTC_E_NONE;
}

int32
sys_tmm_qos_shape_init(uint8 lchip)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    QMgrCtl_m  qmgr_ctl;
    QMgrShpCtl_m qmgr_shp_ctl;
    RefDivQMgrDeqShpPulse_m shp_pulse;
    sys_qos_rate_granularity_t shp_gran[SYS_SHP_GRAN_RANAGE_NUM] = {{ 2,       10} ,
                                                                    {100,      40},
                                                                    {1000,     80},
                                                                    {10000,    500},
                                                                    {40000,    1000},
                                                                    {100000,   1600},
                                                                    {200000,   3200},
                                                                    {400000,   6400}};
    sal_memset(&qmgr_ctl, 0, sizeof(QMgrCtl_m));
    sal_memset(&qmgr_shp_ctl, 0, sizeof(QMgrShpCtl_m));
    sal_memset(&shp_pulse, 0, sizeof(RefDivQMgrDeqShpPulse_m));

    sal_memcpy(p_usw_queue_master[lchip]->que_shp_gran, shp_gran,
               SYS_SHP_GRAN_RANAGE_NUM * sizeof(sys_qos_rate_granularity_t));

    CTC_ERROR_RETURN(_sys_tmm_shape_init_shp_ctl(lchip)); /*must config first,bug112782*/
    CTC_ERROR_RETURN(_sys_tmm_shape_init_queue_shp(lchip));
    CTC_ERROR_RETURN(_sys_tmm_shape_init_group_shp(lchip));
    CTC_ERROR_RETURN(_sys_tmm_shape_init_chan_shp(lchip));
    CTC_ERROR_RETURN(sys_tmm_qos_set_reason_shp_base_pkt_en(lchip, TRUE));

    cmd = DRV_IOR(QMgrCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &qmgr_ctl));
    SetQMgrCtl(V, cosShpEn_f, &qmgr_ctl, 0);
    SetQMgrCtl(V, ermInfoEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, cosShpEn0_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, ctlQueAllHigh_f, &qmgr_ctl, 0);
    SetQMgrCtl(V, disableWeightShiftEstimate_f, &qmgr_ctl, 0);
    SetQMgrCtl(V, grpMode_f, &qmgr_ctl, 0xFF);
    SetQMgrCtl(V, grpNodeShpEn0_f, &qmgr_ctl, 0);
    SetQMgrCtl(V, grpNodeShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, grpShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l0NodeShpEn0_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l0NodeShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l0ShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l1NodeShpEn0_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l1NodeShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l1ShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l2NodeShpEn0_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l2NodeShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l2ShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l3NodeShpEn0_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l3NodeShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, l3ShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, portShpEn_f, &qmgr_ctl, 1);
    SetQMgrCtl(V, queShpEn_f, &qmgr_ctl, 1);
    cmd = DRV_IOW(QMgrCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &qmgr_ctl));

    cmd = DRV_IOR(QMgrShpCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &qmgr_shp_ctl));
    SetQMgrShpCtl(V, gran_0_pktLenUnit_f, &qmgr_shp_ctl, 160000);
    SetQMgrShpCtl(V, gran_1_pktLenUnit_f, &qmgr_shp_ctl, 80000);
    SetQMgrShpCtl(V, gran_2_pktLenUnit_f, &qmgr_shp_ctl, 40000);
    SetQMgrShpCtl(V, gran_3_pktLenUnit_f, &qmgr_shp_ctl, 20000);
    SetQMgrShpCtl(V, gran_4_pktLenUnit_f, &qmgr_shp_ctl, 10000);
    SetQMgrShpCtl(V, gran_5_pktLenUnit_f, &qmgr_shp_ctl, 5000);
    SetQMgrShpCtl(V, gran_6_pktLenUnit_f, &qmgr_shp_ctl, 1000);
    SetQMgrShpCtl(V, gran_7_pktLenUnit_f, &qmgr_shp_ctl, 200);
    cmd = DRV_IOW(QMgrShpCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &qmgr_shp_ctl));

    field_val = 0;
    cmd = DRV_IOW(QMgrDeqMiscCtl_t, QMgrDeqMiscCtl_cfgShpRePickUseGrpAvailEn_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));
    cmd = DRV_IOW(QMgrDeqMiscCtl_t, QMgrDeqMiscCtl_cfgShpRePickUseShpStateEn_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, 0, cmd, &field_val));

    p_usw_queue_master[lchip]->que_shp_update_freq = 160000;
    p_usw_queue_master[lchip]->chan_shp_update_freq = 160000;

    /*Freq: 256k, pulse:0x4c3b4,Freq: 160k, pulse:0x3cf90
     *((0x3cf + 1) + 0x90 / 256) * 160000 = 156.25 * 1000000
     */
    SetRefDivQMgrDeqShpPulse(V, cfgRefDivQMgrDeqShpPulse_f, &shp_pulse, 0x3cf90);
    SetRefDivQMgrDeqShpPulse(V, cfgResetDivQMgrDeqShpPulse_f, &shp_pulse, 0);
    cmd = DRV_IOW(RefDivQMgrDeqShpPulse_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &shp_pulse));

    return CTC_E_NONE;
}



int32
_sys_tmm_shape_qos_get_shp_gran(uint8 lchip, uint32 rate, /*kb/s*/
                                                       uint8 is_pps,
                                                       uint8 *gran_lvl,
                                                       uint32 *sw_gran,
                                                       uint32 *bucket_cnt_gran)
{
    uint8 index = 0;
    uint32 token_rate_Mbps = rate / 1000;

    /*per byte use token num
    * rate gran : update_freq / byte_bucket_cnt_gran (byte/s)
    * thrd gran : (1 << 16) / byte_bucket_cnt_gran (byte)
    */
    uint32 byte_bucket_cnt_gran[8] = {256,256,256,128,32,8,4,2};
    uint32 byte_bucket_cnt_gran_lvl[8] = {0,0,0,1,3,5,6,7};

    /*per pkt use token num
    * pps rate gran : update_freq / pkt_bucket_cnt_gran (pps)
    * pps thrd gran : (1 << 16) / pkt_bucket_cnt_gran (pkt)
    */
    uint32 pkt_bucket_cnt_gran[8] = {160000,80000,40000,20000,10000,5000,1000,200};

    /*max pps rate per rate gran*/
    uint32 rate_gran_interval_pps[8] = {1000000,2000000,4000000,8000000,16000000,32000000,160000000,800000000};

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    for (index = 0; index < 8; index++)
    {
        if (is_pps && (rate <= rate_gran_interval_pps[index]))
        {
            *gran_lvl = index;
            *bucket_cnt_gran = pkt_bucket_cnt_gran[index];
            break;
        }
        else if (!is_pps && token_rate_Mbps <= p_usw_queue_master[lchip]->que_shp_gran[index].max_rate)
        {
            *sw_gran = p_usw_queue_master[lchip]->que_shp_gran[index].granularity;
            *gran_lvl = byte_bucket_cnt_gran_lvl[index];
            *bucket_cnt_gran = byte_bucket_cnt_gran[index];
            break;
        }
    }

    return CTC_E_NONE;
}


int32
sys_tmm_shape_map_profile_user_to_hw(uint8 lchip, uint32 user_rate, uint32 user_bucket_thrd,
                                                              uint8 is_pps, uint8 *gran_lvl, uint32 *hw_rate, uint32 *hw_bucket_thrd)
{
    uint32 bucket_cnt_gran = 1;
    uint32 sw_gran = 1;
    uint64 bucket_thrd = 0;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    _sys_tmm_shape_qos_get_shp_gran(lchip, user_rate, is_pps, gran_lvl, &sw_gran, &bucket_cnt_gran);

    user_rate  = (user_rate / sw_gran) * sw_gran;
    if (is_pps)
    {
       *hw_rate = user_rate / (p_usw_queue_master[lchip]->que_shp_update_freq / bucket_cnt_gran);
    }
    else
    {
        *hw_rate = user_rate / (p_usw_queue_master[lchip]->que_shp_update_freq / bucket_cnt_gran * 8 / 1000);
    }
    if (0 == user_bucket_thrd)
    {
        bucket_thrd = (*hw_rate / bucket_cnt_gran + SYS_QOS_MTU) * 5;
    }
    else
    {
        bucket_thrd = is_pps ? user_bucket_thrd : user_bucket_thrd * 125;
    }

    bucket_thrd = (bucket_thrd * bucket_cnt_gran) >> 16;
    *hw_bucket_thrd = ((*hw_rate == 0) && (0 == user_bucket_thrd)) ? 0 : (bucket_thrd == 0 ? 1 : bucket_thrd);

    if(*hw_rate > MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE))
    {
        *hw_rate = MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE);
    }

    if(*hw_bucket_thrd > MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD))
    {
        *hw_bucket_thrd = MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD);
    }

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, " is_pps : %d\n",  is_pps);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, " gran_lvl : %d\n",  *gran_lvl);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, " hw_rate : %d\n",  *hw_rate);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, " hw_bucket_thrd : %d\n",  *hw_bucket_thrd);

    return CTC_E_NONE;
}

int32
sys_tmm_shape_map_profile_hw_to_user(uint8 lchip, uint32 hw_rate, uint32 hw_bucket_thrd,
                                                              uint8 is_pps, uint8 gran_lvl, uint32 *user_rate, uint32 *user_bucket_thrd)
{
    uint32 bucket_cnt_gran = 0;
    uint32 byte_bucket_cnt_gran[8] = {256,128,64,32,16,8,4,2};
    uint32 pkt_bucket_cnt_gran[8] = {160000,80000,40000,20000,10000,5000,1000,200};

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    bucket_cnt_gran = is_pps ? pkt_bucket_cnt_gran[gran_lvl] : byte_bucket_cnt_gran[gran_lvl];

    if (is_pps)
    {
        *user_rate = hw_rate * (p_usw_queue_master[lchip]->que_shp_update_freq / bucket_cnt_gran);
        *user_bucket_thrd = (hw_bucket_thrd << 16) / bucket_cnt_gran;
    }
    else
    {
        *user_rate = hw_rate * (p_usw_queue_master[lchip]->que_shp_update_freq / bucket_cnt_gran * 8 / 1000);
        *user_bucket_thrd = (hw_bucket_thrd << 16) / bucket_cnt_gran * 8 / 1000;
    }

    return CTC_E_NONE;
}

int32
sys_tmm_shape_add_que_shp_to_asic(uint8 lchip, uint16 queue_id, uint8 queue_offset,
                                                        void* p_shape, uint8 is_pps)
{
    uint32 cmd = 0;
    uint32 token_rate = 0;
    uint32 token_threshold = 0;
    uint32 token_rate_cir = 0;
    uint32 token_threshold_cir = 0;
    uint16 sub_grp_id = 0;
    uint16 chan_id;
    uint8  gran_lvl = 0;
    uint8  gran_lvl_cir = 0;
    uint8  prof_step = 0;
    uint8  token_step = 0;
    uint32 group_index = 0;
    uint32 value = 0;
    uint32 table_id = 0;
    uint32 token_table_id = 0;
    uint32 token_field_id = 0;
    ctc_qos_shape_queue_t* p_que_shape = (ctc_qos_shape_queue_t*)p_shape;
    DsQMgrQueShpProfile0_m que_shp_profile;
    DsQMgrCosGrpShpProfile0_m cos_shp_profile;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&que_shp_profile, 0, sizeof(DsQMgrQueShpProfile0_m));
    sal_memset(&cos_shp_profile, 0, sizeof(DsQMgrCosGrpShpProfile0_m));

    SYS_TMM_GROUP_ID(sub_grp_id, queue_id, 1);
    SYS_TMM_GROUP_INDEX(group_index, sub_grp_id);

    /*get rate*/
    if (p_que_shape->enable)
    {
        sys_tmm_shape_map_profile_user_to_hw(lchip, p_que_shape->pir, p_que_shape->pbs,
                                             is_pps, &gran_lvl, &token_rate, &token_threshold);
        sys_tmm_shape_map_profile_user_to_hw(lchip, p_que_shape->cir, p_que_shape->cbs,
                                             is_pps, &gran_lvl_cir, &token_rate_cir, &token_threshold_cir);
    }
    else
    {
        token_rate = MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE);
        token_threshold = MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD);
        gran_lvl = 7;
        token_rate_cir = 0;
        token_threshold_cir= 0;
        gran_lvl_cir= 0;
    }

    if (queue_id < MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_NETWORK_MISC))
    {
        /*pir*/
	    prof_step = DsQMgrQueShpProfile0_g_1_shpProfile_tokenRate_f - DsQMgrQueShpProfile0_g_0_shpProfile_tokenRate_f;
        token_step = DsQMgrQueShpToken0_g_1_shpToken_token_f - DsQMgrQueShpToken0_g_0_shpToken_token_f;

        table_id = CTC_IS_BIT_SET(queue_id, 3) ? DsQMgrQueShpProfile1_t : DsQMgrQueShpProfile0_t;
        token_table_id = CTC_IS_BIT_SET(queue_id, 3) ? DsQMgrQueShpToken1_t : DsQMgrQueShpToken0_t;
        token_field_id = CTC_IS_BIT_SET(queue_id, 3) ? (DsQMgrQueShpToken1_g_0_shpToken_token_f + token_step * queue_offset)
                                                 : (DsQMgrQueShpToken0_g_0_shpToken_token_f + token_step * queue_offset);

        cmd = DRV_IOR(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, group_index, cmd, &que_shp_profile));
        SetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenRate_f + prof_step * queue_offset, &que_shp_profile, token_rate);
        SetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenThrd_f + prof_step * queue_offset, &que_shp_profile, token_threshold);
        SetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenGran_f + prof_step * queue_offset, &que_shp_profile, gran_lvl);
        SetDsQMgrQueShpProfile0(V, g_0_shpProfile_ppsMode_f + prof_step * queue_offset, &que_shp_profile, is_pps);
        cmd = DRV_IOW(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, group_index, cmd, &que_shp_profile));

        cmd = DRV_IOW(token_table_id, token_field_id);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_index, cmd, &value));

        /*cir*/
        prof_step = DsQMgrCosGrpShpProfile0_g_1_shpProfile0_tokenGran_f - DsQMgrCosGrpShpProfile0_g_0_shpProfile0_tokenGran_f;
        token_step = DsQMgrCosGrpShpToken0_g_1_shpToken0_token_f - DsQMgrCosGrpShpToken0_g_0_shpToken0_token_f;

        cmd = DRV_IOR(DsQMgrCosGrpShpProfile0_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, sub_grp_id, cmd, &cos_shp_profile));
        SetDsQMgrCosGrpShpProfile0(V, g_0_shpProfile0_tokenGran_f + queue_offset * prof_step, &cos_shp_profile, gran_lvl_cir);
        SetDsQMgrCosGrpShpProfile0(V, g_0_shpProfile0_tokenRate_f + queue_offset * prof_step, &cos_shp_profile, token_rate_cir);
        SetDsQMgrCosGrpShpProfile0(V, g_0_shpProfile0_tokenThrd_f + queue_offset * prof_step, &cos_shp_profile, token_threshold_cir);
        SetDsQMgrCosGrpShpProfile0(V, g_0_shpProfile0_ppsMode_f + queue_offset * prof_step, &cos_shp_profile, is_pps);
        cmd = DRV_IOW(DsQMgrCosGrpShpProfile0_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, sub_grp_id, cmd, &cos_shp_profile));

        cmd = DRV_IOW(DsQMgrCosGrpShpToken0_t, DsQMgrCosGrpShpToken0_g_0_shpToken0_token_f + token_step * queue_offset);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_grp_id, cmd, &value));
    }
    else
    {
        /* cfg ctl queue shape,only pir*/
        DsQMgrCtlQueShpProfile_m ctl_queue_profile;

        sal_memset(&ctl_queue_profile, 0, sizeof(DsQMgrCtlQueShpProfile_m));

        CTC_ERROR_RETURN(sys_tmm_queue_get_channel_by_queue_id(lchip, queue_id, &chan_id));
        prof_step = DsQMgrCtlQueShpProfile_g_1_shpProfile_tokenGran_f - DsQMgrCtlQueShpProfile_g_0_shpProfile_tokenGran_f;
        token_step = DsQMgrCtlQueShpToken_g_1_shpToken_token_f - DsQMgrCtlQueShpToken_g_0_shpToken_token_f;

        cmd = DRV_IOR(DsQMgrCtlQueShpProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &ctl_queue_profile));
        SetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenGran_f + queue_offset * prof_step, &ctl_queue_profile, gran_lvl);
        SetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenRate_f + queue_offset * prof_step, &ctl_queue_profile, token_rate);
        SetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenThrd_f + queue_offset * prof_step, &ctl_queue_profile, token_threshold);
        SetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_ppsMode_f + queue_offset * prof_step, &ctl_queue_profile, is_pps);
        cmd = DRV_IOW(DsQMgrCtlQueShpProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &ctl_queue_profile));

        cmd = DRV_IOW(DsQMgrCtlQueShpToken_t, DsQMgrCtlQueShpToken_g_0_shpToken_token_f + token_step * queue_offset);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, chan_id, cmd, &value));
    }

    return CTC_E_NONE;
}

int32
sys_tmm_shape_add_chan_shp_to_asic(uint8 lchip, uint16 channel, uint32 pir, uint32 pbs,
                                                           uint8 is_pps, uint8 enable)
{
    uint32 cmd = 0;
    uint32 token_rate = 0;
    uint32 token_threshold = 0;
    uint32 value = 0;
    uint8  gran_lvl = 0;
    DsQMgrPortShpProfile_m port_shp_profile;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&port_shp_profile, 0, sizeof(DsQMgrPortShpProfile_m));

    sys_tmm_shape_map_profile_user_to_hw(lchip, pir, pbs, is_pps, &gran_lvl, &token_rate, &token_threshold);

    cmd = DRV_IOR(DsQMgrPortShpProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, channel, cmd, &port_shp_profile));

    if (!enable)
    {
        token_rate = MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE);
        token_threshold = MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD);
        gran_lvl = 7;
    }

    SetDsQMgrPortShpProfile(V, ppsMode_f, &port_shp_profile, is_pps);
    SetDsQMgrPortShpProfile(V, tokenGran_f  , &port_shp_profile, gran_lvl);
    SetDsQMgrPortShpProfile(V, tokenRate_f, &port_shp_profile, token_rate);
    SetDsQMgrPortShpProfile(V, tokenThrd_f  , &port_shp_profile, token_threshold);

    cmd = DRV_IOW(DsQMgrPortShpProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, channel, cmd, &port_shp_profile));

    cmd = DRV_IOW(DsQMgrPortShpToken_t, DsQMgrPortShpToken_token_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, channel, cmd, &value));

    return CTC_E_NONE;
}


int32
sys_tmm_shape_get_port_shape_profile(uint8 lchip, uint16 channel, uint32* rate, uint32* thrd,
                                                              uint8* pps_en, uint8* p_shp_en)
{
    uint32 cmd;
    uint32 token_thrd = 0;
    uint8  token_gran = 0;
    uint32 token_rate = 0;
    uint8  is_pps = 0;
    DsQMgrPortShpProfile_m  port_shp_profile;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&port_shp_profile, 0, sizeof(DsQMgrPortShpProfile_m));

    cmd = DRV_IOR(DsQMgrPortShpProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, channel, cmd, &port_shp_profile));

    is_pps = GetDsQMgrPortShpProfile(V, ppsMode_f, &port_shp_profile);
    token_gran = GetDsQMgrPortShpProfile(V, tokenGran_f, &port_shp_profile);
    token_rate = GetDsQMgrPortShpProfile(V, tokenRate_f, &port_shp_profile);
    token_thrd = GetDsQMgrPortShpProfile(V, tokenThrd_f, &port_shp_profile);

    *p_shp_en = (token_rate != MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE)) || (token_thrd != MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD))
                || (token_gran != 7);

    *pps_en = is_pps;

    sys_tmm_shape_map_profile_hw_to_user(lchip, token_rate, token_thrd, is_pps, token_gran, rate, thrd);

    return CTC_E_NONE;
}

int32
sys_tmm_shape_get_que_shape_profile(uint8 lchip, uint16 queue_id, uint16 queue_offset, void* p_shape)
{

    uint32 cmd = 0;
    uint16 sub_grp_id = 0;
    uint8  gran_lvl = 0;
    uint8  gran_lvl_cir = 0;
    uint8  prof_step = 0;
    uint32 group_index = 0;
    uint32 table_id = 0;
    uint8  shp_en = 0;
    uint8  is_pps = 0;
    uint32 token_rate = 0;
    uint32 token_thrd = 0;
    uint8  shp_en_cir = 0;
    uint8  is_pps_cir = 0;
    uint32 token_rate_cir = 0;
    uint32 token_thrd_cir = 0;

    ctc_qos_shape_queue_t* p_que_shape = (ctc_qos_shape_queue_t*)p_shape;

    if (queue_id < MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_NETWORK_MISC))
    {
        DsQMgrQueShpProfile0_m que_shp_profile;
        DsQMgrCosGrpShpProfile0_m cos_shp_profile;
        sal_memset(&que_shp_profile, 0, sizeof(DsQMgrQueShpProfile0_m));
        sal_memset(&cos_shp_profile, 0, sizeof(DsQMgrCosGrpShpProfile0_m));

        SYS_TMM_GROUP_ID(sub_grp_id, queue_id, 1);
        SYS_TMM_GROUP_INDEX(group_index, sub_grp_id);
        /*pir*/
        prof_step = DsQMgrQueShpProfile0_g_1_shpProfile_tokenRate_f - DsQMgrQueShpProfile0_g_0_shpProfile_tokenRate_f;
        table_id = CTC_IS_BIT_SET(queue_id, 3) ? DsQMgrQueShpProfile1_t : DsQMgrQueShpProfile0_t;

        cmd = DRV_IOR(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, group_index, cmd, &que_shp_profile));
        token_rate = GetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenRate_f + prof_step * queue_offset, &que_shp_profile);
        token_thrd = GetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenThrd_f + prof_step * queue_offset, &que_shp_profile);
        gran_lvl = GetDsQMgrQueShpProfile0(V, g_0_shpProfile_tokenGran_f + prof_step * queue_offset, &que_shp_profile);
        is_pps = GetDsQMgrQueShpProfile0(V, g_0_shpProfile_ppsMode_f + prof_step * queue_offset, &que_shp_profile);

        /*cir*/
        cmd = DRV_IOR(DsQMgrCosGrpShpProfile0_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, sub_grp_id, cmd, &cos_shp_profile));
        token_rate_cir = GetDsQMgrCosGrpShpProfile0(V, g_0_shpProfile0_tokenRate_f + prof_step * queue_offset, &cos_shp_profile);
        token_thrd_cir = GetDsQMgrCosGrpShpProfile0(V, g_0_shpProfile0_tokenThrd_f + prof_step * queue_offset, &cos_shp_profile);
        gran_lvl_cir = GetDsQMgrCosGrpShpProfile0(V, g_0_shpProfile0_tokenGran_f + prof_step * queue_offset, &cos_shp_profile);
        is_pps_cir = GetDsQMgrCosGrpShpProfile0(V, g_0_shpProfile0_ppsMode_f + prof_step * queue_offset, &cos_shp_profile);
    }
    else
    {
        /*ctl queue, only pir*/
        uint16 chan_id = 0;
        DsQMgrCtlQueShpProfile_m ctl_queue_profile;
        sal_memset(&ctl_queue_profile, 0, sizeof(DsQMgrCtlQueShpProfile_m));
        CTC_ERROR_RETURN(sys_tmm_queue_get_channel_by_queue_id(lchip, queue_id, &chan_id));

        prof_step = DsQMgrCtlQueShpProfile_g_1_shpProfile_tokenGran_f - DsQMgrCtlQueShpProfile_g_0_shpProfile_tokenGran_f;

        cmd = DRV_IOR(DsQMgrCtlQueShpProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &ctl_queue_profile));
        gran_lvl = GetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenGran_f + queue_offset * prof_step, &ctl_queue_profile);
        token_rate = GetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenRate_f + queue_offset * prof_step, &ctl_queue_profile);
        token_thrd = GetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_tokenThrd_f + queue_offset * prof_step, &ctl_queue_profile);
        is_pps = GetDsQMgrCtlQueShpProfile(V, g_0_shpProfile_ppsMode_f + queue_offset * prof_step, &ctl_queue_profile);
    }

    shp_en = (token_rate != MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE)) || (token_thrd != MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD))
                || (gran_lvl != 7);
    shp_en_cir = (token_rate_cir != 0) || (token_thrd_cir != 0) || (gran_lvl_cir != 0);
    p_que_shape->enable = shp_en || shp_en_cir;
    p_que_shape->pps_en = (is_pps && shp_en) || (is_pps_cir && shp_en_cir);

    sys_tmm_shape_map_profile_hw_to_user(lchip, token_rate, token_thrd, is_pps, gran_lvl, &p_que_shape->pir, &p_que_shape->pbs);
    sys_tmm_shape_map_profile_hw_to_user(lchip, token_rate_cir, token_thrd_cir, is_pps_cir, gran_lvl_cir, &p_que_shape->cir, &p_que_shape->cbs);

    return CTC_E_NONE;
}


int32
sys_tmm_shape_get_grp_shape_profile(uint8 lchip, uint32 group_id, uint32* rate, uint32* thrd,
                                                             uint8* pps_en, uint8* p_shp_en)
{

    uint32 cmd = 0;
    uint32 token_rate = 0;
    uint32 token_thrd = 0;
    uint32 tbl_index = 0;
    uint32 gran_lvl = 0;
    uint8  is_pps = 0;
    uint16 sub_grp_id = 0;
    uint8  level = 0;
    DsQMgrGrpShpProfile_m grp_shp_profile;
    uint32 grp_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0ShpProfile_t, DsQMgrL1ShpProfile_t,
                                                    DsQMgrL2ShpProfile_t, DsQMgrL3ShpProfile_t,
                                                    DsQMgrGrpShpProfile_t};

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&grp_shp_profile, 0, sizeof(DsQMgrGrpShpProfile_m));
    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);

    tbl_index  = sub_grp_id;

    cmd = DRV_IOR(grp_tbl_id[level], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &grp_shp_profile));

    token_rate = GetDsQMgrGrpShpProfile(V, tokenRate_f, &grp_shp_profile);
    token_thrd = GetDsQMgrGrpShpProfile(V, tokenThrd_f, &grp_shp_profile);
    gran_lvl = GetDsQMgrGrpShpProfile(V, tokenGran_f, &grp_shp_profile);
    is_pps = GetDsQMgrGrpShpProfile(V, ppsMode_f, &grp_shp_profile);

    *p_shp_en = (token_rate != MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE)) || (token_thrd != MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD))
                || (gran_lvl != 7);
    *pps_en = is_pps;

    sys_tmm_shape_map_profile_hw_to_user(lchip, token_rate, token_thrd, is_pps, gran_lvl, rate, thrd);

    return CTC_E_NONE;
}

int32
sys_tmm_shape_get_grp_node_shape_profile(uint8 lchip, uint32 group_id, uint8 group_node,
                                                                     uint8 is_cir, uint32* rate, uint32* thrd,
                                                                     uint8* pps_en, uint8* p_shp_en)
{
    uint32 cmd = 0;
    uint32 token_rate = 0;
    uint32 token_thrd = 0;
    uint32 tbl_index = 0;
    uint32 table_id = 0;
    uint32 gran_lvl = 0;
    uint8  is_pps = 0;
    uint16 sub_grp_id = 0;
    uint8  level = 0;
    uint8  step_node = 0;
    uint8  step_pir = 0;
    uint8  step = 0;
    DsQMgrL0NodeShpProfile_m grp_node_shp_profile;
    DsQMgrGrpNodeShpProfile_m que_grp_node_shp_profile;
    uint32 node_pir_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpProfile_t, DsQMgrL1NodeShpProfile_t,
                                                         DsQMgrL2NodeShpProfile_t, DsQMgrL3NodeShpProfile_t,
                                                         DsQMgrGrpNodeShpProfile_t};
    uint32 node_cir_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpProfile0_t, DsQMgrL1NodeShpProfile0_t,
                                                         DsQMgrL2NodeShpProfile0_t, DsQMgrL3NodeShpProfile0_t,
                                                         DsQMgrGrpNodeShpProfile_t};

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&grp_node_shp_profile, 0, sizeof(DsQMgrL0NodeShpProfile_m));
    sal_memset(&que_grp_node_shp_profile, 0, sizeof(DsQMgrGrpNodeShpProfile_m));

    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);
    SYS_CHECK_LVL_GROUP_NODE(group_node, level);

    tbl_index  = sub_grp_id;
    table_id = is_cir ? node_cir_tbl_id[level] : node_pir_tbl_id[level];

    if (CTC_QOS_SCHED_GROUP_LVL_Q == level)
    {
        cmd = DRV_IOR(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &que_grp_node_shp_profile));
        step_node = DsQMgrGrpNodeShpProfile_g_1_shpProfile0_ppsMode_f - DsQMgrGrpNodeShpProfile_g_0_shpProfile0_ppsMode_f;
        step_pir = DsQMgrGrpNodeShpProfile_g_0_shpProfile_ppsMode_f - DsQMgrGrpNodeShpProfile_g_0_shpProfile0_ppsMode_f;
        step = group_node * step_node + (is_cir ? 0 : step_pir);
        token_rate = GetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile0_tokenRate_f + step, &que_grp_node_shp_profile);
        token_thrd = GetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile0_tokenThrd_f + step, &que_grp_node_shp_profile);
        gran_lvl = GetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile0_tokenGran_f + step, &que_grp_node_shp_profile);
        is_pps = GetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile0_ppsMode_f + step, &que_grp_node_shp_profile);
    }
    else
    {
        cmd = DRV_IOR(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &grp_node_shp_profile));
        step_node = DsQMgrL0NodeShpProfile_g_1_shpProfile_tokenRate_f - DsQMgrL0NodeShpProfile_g_0_shpProfile_tokenRate_f;
        step = group_node * step_node;
        token_rate = GetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenRate_f + step, &grp_node_shp_profile);
        token_thrd = GetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenThrd_f + step, &grp_node_shp_profile);
        gran_lvl = GetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenGran_f + step, &grp_node_shp_profile);
        is_pps = GetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_ppsMode_f + step, &grp_node_shp_profile);
    }

    if (is_cir)
    {
        *p_shp_en = token_rate != 0 || token_thrd != 0 || gran_lvl != 0;
    }
    else
    {
        *p_shp_en = (token_rate != MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE)) || (token_thrd != MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD))
                    || (gran_lvl != 7);
    }
    *pps_en = is_pps;

    sys_tmm_shape_map_profile_hw_to_user(lchip, token_rate, token_thrd, is_pps, gran_lvl, rate, thrd);

    return CTC_E_NONE;
}

int32
sys_tmm_shape_add_group_shape(uint8 lchip, uint32 group_id, uint8 is_pps,
                                                    uint32 pir, uint32 pbs, uint8 enable)
{
    uint32 cmd = 0;
    uint32 token_rate = 0;
    uint32 token_threshold = 0;
    uint32 tbl_index = 0;
    uint32 value = 0;
    uint8 gran_lvl = 0;
    uint16 sub_grp_id = 0;
    uint8  level = 0;
    DsQMgrGrpShpProfile_m grp_shp_profile;
    uint32 grp_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0ShpProfile_t, DsQMgrL1ShpProfile_t,
                                                    DsQMgrL2ShpProfile_t, DsQMgrL3ShpProfile_t,
                                                    DsQMgrGrpShpProfile_t};
    uint32 token_table_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0ShpToken_t, DsQMgrL1ShpToken_t,
                                                        DsQMgrL2ShpToken_t, DsQMgrL3ShpToken_t,
                                                        DsQMgrGrpShpToken_t};

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&grp_shp_profile, 0, sizeof(DsQMgrGrpShpProfile_m));
    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);

    sys_tmm_shape_map_profile_user_to_hw(lchip, pir, pbs, is_pps, &gran_lvl, &token_rate, &token_threshold);

    tbl_index  = sub_grp_id;

    cmd = DRV_IOR(grp_tbl_id[level], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &grp_shp_profile));

    if (!enable)
    {
        token_rate = MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE);
        token_threshold = MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD);
        gran_lvl = 7;
    }

    SetDsQMgrGrpShpProfile(V, tokenRate_f, &grp_shp_profile, token_rate);
    SetDsQMgrGrpShpProfile(V, tokenThrd_f, &grp_shp_profile, token_threshold);
    SetDsQMgrGrpShpProfile(V, tokenGran_f, &grp_shp_profile, gran_lvl);
    SetDsQMgrGrpShpProfile(V, ppsMode_f, &grp_shp_profile, is_pps);

    cmd = DRV_IOW(grp_tbl_id[level], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &grp_shp_profile));

    cmd = DRV_IOW(token_table_id[level], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &value));

    return CTC_E_NONE;
}


int32
sys_tmm_shape_add_group_node_shape(uint8 lchip, uint32 group_id, uint8 group_node, uint8 is_pps,
                                                           uint32 pir, uint32 pbs, uint8 is_cir, uint8 enable)
{
    uint32 cmd = 0;
    uint32 token_rate = 0;
    uint32 token_threshold = 0;
    uint32 tbl_index = 0;
    uint32 value = 0;
    uint32 table_id = 0;
    uint32 token_table_id = 0;
    uint8  gran_lvl = 0;
    uint16 sub_grp_id = 0;
    uint8  level = 0;
    uint8  step_node = 0;
    uint8  step_pir = 0;
    uint8  step = 0;
    uint32 field_id = 0;
    DsQMgrL0NodeShpProfile_m grp_node_shp_profile;
    DsQMgrGrpNodeShpProfile_m que_grp_node_shp_profile;
    DsQMgrL0NodeShpToken_m grp_node_shp_token;
    DsQMgrGrpNodeShpToken_m que_grp_node_shp_token;
    uint32 node_pir_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpProfile_t, DsQMgrL1NodeShpProfile_t,
                                                         DsQMgrL2NodeShpProfile_t, DsQMgrL3NodeShpProfile_t,
                                                         DsQMgrGrpNodeShpProfile_t};
    uint32 node_cir_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpProfile0_t, DsQMgrL1NodeShpProfile0_t,
                                                         DsQMgrL2NodeShpProfile0_t, DsQMgrL3NodeShpProfile0_t,
                                                         DsQMgrGrpNodeShpProfile_t};
    uint32 node_pir_token_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpToken_t, DsQMgrL1NodeShpToken_t,
                                                               DsQMgrL2NodeShpToken_t, DsQMgrL3NodeShpToken_t,
                                                               DsQMgrGrpNodeShpToken_t};
    uint32 node_cir_token_tbl_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0NodeShpToken0_t, DsQMgrL1NodeShpToken0_t,
                                                               DsQMgrL2NodeShpToken0_t, DsQMgrL3NodeShpToken0_t,
                                                               DsQMgrGrpNodeShpToken_t};

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    if ((CTC_QOS_SCHED_GROUP_LVL_Q == level) && is_cir && pir)
    {
        return CTC_E_NOT_SUPPORT;
    }
    SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);
    SYS_CHECK_LVL_GROUP_NODE(group_node, level);

    sal_memset(&grp_node_shp_profile, 0, sizeof(DsQMgrL0NodeShpProfile_m));
    sal_memset(&que_grp_node_shp_profile, 0, sizeof(DsQMgrGrpNodeShpProfile_m));
    sal_memset(&grp_node_shp_token, 0, sizeof(DsQMgrL0NodeShpToken_m));
    sal_memset(&que_grp_node_shp_token, 0, sizeof(DsQMgrGrpNodeShpToken_m));

    sys_tmm_shape_map_profile_user_to_hw(lchip, pir, pbs, is_pps, &gran_lvl, &token_rate, &token_threshold);

    tbl_index  = sub_grp_id;
    table_id = is_cir ? node_cir_tbl_id[level] : node_pir_tbl_id[level];
    token_table_id = is_cir ? node_cir_token_tbl_id[level] : node_pir_token_tbl_id[level];

    if (!enable)
    {
        token_rate = is_cir ? 0 : MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_RATE);
        token_threshold = is_cir ? 0 : MCHIP_CAP(SYS_CAP_QOS_SHP_TOKEN_THRD);
        gran_lvl = is_cir ? 0 : 7;
    }

    if (CTC_QOS_SCHED_GROUP_LVL_Q == level)
    {
        cmd = DRV_IOR(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &que_grp_node_shp_profile));
        step_node = DsQMgrGrpNodeShpProfile_g_1_shpProfile0_ppsMode_f - DsQMgrGrpNodeShpProfile_g_0_shpProfile0_ppsMode_f;
        step_pir = DsQMgrGrpNodeShpProfile_g_0_shpProfile_ppsMode_f - DsQMgrGrpNodeShpProfile_g_0_shpProfile0_ppsMode_f;
        step = group_node * step_node + (is_cir ? 0 : step_pir);
        SetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile0_tokenRate_f + step, &que_grp_node_shp_profile, token_rate);
        SetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile0_tokenThrd_f + step, &que_grp_node_shp_profile, token_threshold);
        SetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile0_tokenGran_f + step, &que_grp_node_shp_profile, gran_lvl);
        SetDsQMgrGrpNodeShpProfile(V, g_0_shpProfile0_ppsMode_f + step, &que_grp_node_shp_profile, is_pps);
        cmd = DRV_IOW(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &que_grp_node_shp_profile));

        cmd = DRV_IOR(token_table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &que_grp_node_shp_token));
        step_node = DsQMgrGrpNodeShpToken_g_1_shpToken0_token_f - DsQMgrGrpNodeShpToken_g_0_shpToken0_token_f;
        step_pir = DsQMgrGrpNodeShpToken_g_0_shpToken_token_f - DsQMgrGrpNodeShpToken_g_0_shpToken0_token_f;
        step = group_node * step_node + (is_cir ? 0 : step_pir);
        SetDsQMgrGrpNodeShpToken(V, g_0_shpToken0_token_f + step, &que_grp_node_shp_token, value);
        cmd = DRV_IOW(token_table_id, field_id);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, tbl_index, cmd, &que_grp_node_shp_token));
    }
    else
    {
        cmd = DRV_IOR(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &grp_node_shp_profile));
        step_node = DsQMgrL0NodeShpProfile_g_1_shpProfile_tokenRate_f - DsQMgrL0NodeShpProfile_g_0_shpProfile_tokenRate_f;
        step = group_node * step_node;
        SetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenRate_f + step, &grp_node_shp_profile, token_rate);
        SetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenThrd_f + step, &grp_node_shp_profile, token_threshold);
        SetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_tokenGran_f + step, &grp_node_shp_profile, gran_lvl);
        SetDsQMgrL0NodeShpProfile(V, g_0_shpProfile_ppsMode_f + step, &grp_node_shp_profile, is_pps);
        cmd = DRV_IOW(table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &grp_node_shp_profile));

        cmd = DRV_IOR(token_table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &grp_node_shp_token));
        step_node = DsQMgrL0NodeShpToken0_g_1_shpToken_token_f - DsQMgrL0NodeShpToken0_g_0_shpToken_token_f;
        step = group_node * step_node;
        SetDsQMgrL0NodeShpToken(V, g_0_shpToken_token_f + step, &grp_node_shp_token, value);
        cmd = DRV_IOW(token_table_id, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &grp_node_shp_token));
    }

    return CTC_E_NONE;
}

#define __SCHEDULE__
int32
_sys_tmm_sch_set_group_chan(uint8 lchip, sys_sch_group_node_t *p_child_sch_group_node,sys_sch_group_node_t *p_parent_sch_group_node)
{
    uint8  level = 0;
    uint32 sub_grp_id = 0;
    uint32 grp_vec_index = 0;
    sys_sch_group_child_node_t *p_sch_group_child = NULL;
    sys_sch_group_node_t *p_sch_group_node = NULL;
    ctc_slistnode_t* ctc_slistnode = NULL;

    if (p_child_sch_group_node->chan_valid != 0 && p_parent_sch_group_node->chan_valid != 0
        && p_child_sch_group_node->chan_id != p_parent_sch_group_node->chan_id)
    {
        return CTC_E_INVALID_CONFIG;
    }
    else if (p_child_sch_group_node->chan_valid != 0 && p_parent_sch_group_node->chan_valid == 0)
    {
        p_parent_sch_group_node->chan_valid = 1;
        p_parent_sch_group_node->chan_id = p_child_sch_group_node->chan_id;
        if(p_parent_sch_group_node->parent_id)
        {
            SYS_LVL_GRP_TO_SUB_GRP(p_parent_sch_group_node->parent_id, level, sub_grp_id);
            SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, level, sub_grp_id);
            p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
            if (NULL == p_sch_group_node)
            {
                SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
                return CTC_E_NOT_EXIST;
            }
            CTC_ERROR_RETURN(_sys_tmm_sch_set_group_chan(lchip, p_parent_sch_group_node, p_sch_group_node));
        }
        else
        {
            CTC_SLIST_LOOP(p_parent_sch_group_node->p_child_list, ctc_slistnode)
            {
                p_sch_group_child = _ctc_container_of(ctc_slistnode, sys_sch_group_child_node_t, head);
                SYS_LVL_GRP_TO_SUB_GRP(p_sch_group_child->child_id, level, sub_grp_id);
                SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, level, sub_grp_id);
                p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
                if (NULL == p_sch_group_node)
                {
                    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
                    return CTC_E_NOT_EXIST;
                }
                CTC_ERROR_RETURN(_sys_tmm_sch_set_group_chan(lchip, p_sch_group_node, p_parent_sch_group_node));
            }
        }
    }
    else if (p_child_sch_group_node->chan_valid == 0 && p_parent_sch_group_node->chan_valid != 0)
    {
        p_child_sch_group_node->chan_valid = 1;
        p_child_sch_group_node->chan_id = p_parent_sch_group_node->chan_id;
        CTC_SLIST_LOOP(p_child_sch_group_node->p_child_list, ctc_slistnode)
        {
            p_sch_group_child = _ctc_container_of(ctc_slistnode, sys_sch_group_child_node_t, head);
            SYS_LVL_GRP_TO_SUB_GRP(p_sch_group_child->child_id, level, sub_grp_id);
            SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, level, sub_grp_id);
            p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
            if (NULL == p_sch_group_node)
            {
                SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
                return CTC_E_NOT_EXIST;
            }
            CTC_ERROR_RETURN(_sys_tmm_sch_set_group_chan(lchip, p_sch_group_node, p_child_sch_group_node));
        }
    }
    return CTC_E_NONE;
}

/*gport mean chan for SYS_EXTEND_QUE_TYPE_C2C*/
int32
_sys_tmm_sch_set_hsched(uint8 lchip, uint32 gport, uint16 group_id_l4, uint8 enable, uint8 type)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint32 group_id = 0;
    uint32 group_id_l0 = 0;
    uint32 group_id_l3 = 0;
    uint32  chan_id = 0;
    uint8  index = 0;
    uint8  is_bind = 0;
    uint8 level = 0;
    uint32 sub_grp_id = 0;
    uint32 grp_vec_index = 0;
    uint32 parent_id = 0;
    sys_sch_group_node_t *p_sch_group_node = NULL;
    sys_usw_dmps_port_info_t dmps_port_info = {0};
    ctc_qos_sched_group_id_t p_sched_group;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    if (SYS_EXTEND_QUE_TYPE_C2C == type)
    {
        chan_id = gport;
    }
    else
    {
        dmps_port_info.gport = gport;
        CTC_ERROR_RETURN(sys_usw_port_api_get_dmps_property(lchip, &dmps_port_info, SYS_PORT_API_DMPS_PROP_PHY_PORT_CHAN_ID, (void*)&chan_id));
        if (!SYS_IS_NETWORK_CHANNEL(chan_id))
        {
            SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Invalid localphy port \n");
            return CTC_E_INVALID_PORT;
        }
    }

    group_id_l3 = p_usw_queue_master[lchip]->l3_group_id_bpe[chan_id];
    if (enable)
    {
        /*chanel has create hsched in bpe mode, only need bind group_id_l4 to group_id_l3*/
        if ((type == SYS_EXTEND_QUE_TYPE_BPE || type == SYS_EXTEND_QUE_TYPE_SERVICE) && group_id_l3)
        {
            SYS_SUB_GRP_TO_LVL_GRP(group_id, CTC_QOS_SCHED_GROUP_LVL_Q, lchip, group_id_l4);
            CTC_ERROR_RETURN(_sys_tmm_sch_create_sched_group(lchip, group_id, group_id_l3, chan_id, 0));
            return CTC_E_NONE;
        }
        CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, chan_id, &is_bind, &group_id_l0));
        if (is_bind)
        {
            SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR,  "channel has bind L0,channel = %d,l0SchId = %d\n", chan_id, group_id_l0);
            return CTC_E_INVALID_CONFIG;
        }

        /*create L0~L4 sch group*/
        for (index = CTC_QOS_SCHED_GROUP_LVL_R; index < CTC_QOS_SCHED_GROUP_LVL_Q; index++)
        {
            uint16 grp_num = 0;
            SYS_QOS_SCH_LVL_GRP_NUM(index, grp_num);
            if (0 == grp_num)
            {
                continue;
            }

            sal_memset(&p_sched_group, 0, sizeof(ctc_qos_sched_group_id_t));

            p_sched_group.level = index;

            if (p_sched_group.level == CTC_QOS_SCHED_GROUP_LVL_R)
            {
                p_sched_group.gport = gport;
            }

            p_sched_group.parent_id = parent_id;

            CTC_ERROR_RETURN(sys_tmm_sch_create_sched_group(lchip, (void*)&p_sched_group, 
                                                           SYS_EXTEND_QUE_TYPE_C2C == type? chan_id: SYS_COMMON_USELESS_CHANNEL));

            parent_id = p_sched_group.group_id;

        }
        SYS_SUB_GRP_TO_LVL_GRP(group_id, CTC_QOS_SCHED_GROUP_LVL_Q, lchip, group_id_l4);
        CTC_ERROR_RETURN(_sys_tmm_sch_create_sched_group(lchip, group_id, parent_id, chan_id, 0));
    }
    else
    {
        if ((type == SYS_EXTEND_QUE_TYPE_BPE || type == SYS_EXTEND_QUE_TYPE_SERVICE) && group_id_l3)
        {
            SYS_SUB_GRP_TO_LVL_GRP(group_id, CTC_QOS_SCHED_GROUP_LVL_Q, lchip, group_id_l4);
            CTC_ERROR_RETURN(_sys_tmm_sch_unbind_sched_group(lchip, group_id, group_id_l3));

            CTC_ERROR_RETURN(sys_tmm_queue_flush(lchip, chan_id, group_id_l4, 0, 0));

            /*del vec and free node*/
            SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, CTC_QOS_SCHED_GROUP_LVL_Q, group_id_l4);
            p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
            if (NULL == p_sch_group_node)
            {
                SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
                return CTC_E_NOT_EXIST;
            }
            ctc_vector_del(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
            SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "ctc_vector_del:%d \n", grp_vec_index);
            if(p_sch_group_node->p_child_list)
            {
                ctc_slist_free(p_sch_group_node->p_child_list);
                p_sch_group_node->p_child_list = NULL;
            }
            mem_free(p_sch_group_node);

            /*should destroy L0~L3 when L4 child is all removed*/
            SYS_LVL_GRP_TO_SUB_GRP(group_id_l3, level, sub_grp_id);
            SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, level, sub_grp_id);
            p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
            if (NULL == p_sch_group_node)
            {
                SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
                return CTC_E_NOT_EXIST;
            }
            if (p_sch_group_node->p_child_list->count)
            {
                return CTC_E_NONE;
            }
        }

        CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, chan_id, &is_bind, &group_id_l0));
        if (!is_bind)
        {
            SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR,  "channel has not bind L0,channel = %d\n", chan_id);
            return CTC_E_NOT_EXIST;
        }

        sal_memset(&p_sched_group, 0, sizeof(ctc_qos_sched_group_id_t));
        p_sched_group.group_id = group_id_l0;
        CTC_ERROR_RETURN(sys_tmm_sch_destroy_sched_group(lchip, (void*)&p_sched_group));
    }

    if (type == SYS_EXTEND_QUE_TYPE_C2C)
    {
        /*config c2c group high prio, counts as setting L0 high prio(0: RR 1: L4 high prio 2: L0 high prio)*/
        field_val = enable ? 2 : 0;
        cmd = DRV_IOW(DsQMgrDeqChanSchCfg_t, DsQMgrDeqChanSchCfg_schMode_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, chan_id, cmd, &field_val));
    }
    else if (type == SYS_EXTEND_QUE_TYPE_BPE || type == SYS_EXTEND_QUE_TYPE_SERVICE)
    {
        p_usw_queue_master[lchip]->l3_group_id_bpe[chan_id] = enable ? parent_id : 0;
        SYS_USW_REGISTER_WB_SYNC_EN(lchip, CTC_FEATURE_QOS, SYS_WB_APPID_QOS_SUBID_QUEUE_MASTER, 1);
    }
    return CTC_E_NONE;
}

int32
_sys_tmm_sch_check_chan_bind_root_group(uint8 lchip, uint8 chan_id, uint8* enable, uint32* group_id)
{
    uint32 cmd = 0;
    uint32 sub_grp_id = 0;
    uint32 parent_id = 0;
    uint32 grp_vec_index = 0;
    uint8  level = CTC_QOS_SCHED_GROUP_LVL_R;
    sys_sch_group_node_t *p_sch_group_node = NULL;

    cmd = DRV_IOR(DsQMgrPortConfig_t, DsQMgrPortConfig_l0SchId_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, chan_id, cmd, &sub_grp_id));

    cmd = DRV_IOR(DsQMgrL0ParentSelect_t, DsQMgrL0ParentSelect_parentId_f);
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_grp_id, cmd, &parent_id));

    SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, level, sub_grp_id);
    p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
    *enable = (parent_id == chan_id && p_sch_group_node != NULL) ? 1 : 0;
    SYS_SUB_GRP_TO_LVL_GRP(*group_id , CTC_QOS_SCHED_GROUP_LVL_R, lchip, sub_grp_id);
    return CTC_E_NONE;
}

int32
_sys_tmm_sch_check_chan_bind_queue_group(uint8 lchip, uint8 chan_id, uint8* enable, uint32* group_id)
{
    DsQMgrGrpParentSelect_m  que_grp_parent;
    DsQMgrPortConfig_m port_cfg;
    uint32 cmd = 0;
    uint16 q_group = 0;
    uint16 parent = 0;
    uint8 link_to_port_en = 0;

    cmd = DRV_IOR(DsQMgrPortConfig_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &port_cfg));
    q_group = GetDsQMgrPortConfig(V, groupSchId_f, &port_cfg);

    cmd = DRV_IOR(DsQMgrGrpParentSelect_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, q_group, cmd, &que_grp_parent));
    parent = GetDsQMgrGrpParentSelect(V, parentId_f, &que_grp_parent);
    link_to_port_en = GetDsQMgrGrpParentSelect(V, linkToPortEn_f, &que_grp_parent);

    *enable = (link_to_port_en && parent == chan_id);
    *group_id = q_group;

    return CTC_E_NONE;
}

int32
_sys_tmm_sch_get_sched_group(uint8 lchip, uint32 child_id, uint32* parent_id)
{
    int32  ret = CTC_E_NONE;
    uint32 child_sub_grp_id = 0;
    uint8  child_level = 0;
    uint32 child_grp_vec_index = 0;
    sys_sch_group_node_t *p_sch_group_node = NULL;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    CTC_PTR_VALID_CHECK(parent_id);
    SYS_LVL_GRP_TO_SUB_GRP(child_id, child_level, child_sub_grp_id);
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(child_grp_vec_index, child_level, child_sub_grp_id);

    p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, child_grp_vec_index);
    if (NULL == p_sch_group_node)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }

    *parent_id = p_sch_group_node->parent_id;

    return ret;
}

int32
_sys_tmm_sch_get_sched_group_child_list(uint8 lchip, uint32 group_id,
                                                                   uint32* child_id_list, uint16* child_cnt)
{
    int32  ret = CTC_E_NONE;
    uint32 sub_grp_id = 0;
    uint8  level = 0;
    uint32 grp_vec_index = 0;
    uint16 index = 0;
    sys_sch_group_node_t *p_sch_group_node = NULL;
    sys_sch_group_child_node_t *p_sch_group_child = NULL;
    ctc_slistnode_t* ctc_slistnode = NULL;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    CTC_PTR_VALID_CHECK(child_id_list);
    CTC_PTR_VALID_CHECK(child_cnt);
    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, level, sub_grp_id);

    p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
    if (NULL == p_sch_group_node)
    {
        *child_cnt = 0;
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }

    CTC_SLIST_LOOP(p_sch_group_node->p_child_list, ctc_slistnode)
    {
        p_sch_group_child = _ctc_container_of(ctc_slistnode, sys_sch_group_child_node_t, head);

        if (index < *child_cnt)
        {
            child_id_list[index++] = p_sch_group_child->child_id;
        }
    }

    *child_cnt = p_sch_group_node->p_child_list->count;

    return ret;
}

/*skip_level = CTC_QOS_SCHED_GROUP_LVL_MAX:skip none, set group and all child
 *skip_level > CTC_QOS_SCHED_GROUP_LVL_MAX:skip all,  set group only
 *skip_level < CTC_QOS_SCHED_GROUP_LVL_MAX:skip level, set group and skip specific level child
 */
int32
_sys_tmm_sch_set_group_config(uint8 lchip, uint32 group_id, uint8 flag,uint8 skip_level, uint8 enable)
{
    int32  ret = CTC_E_NONE;
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint32 sub_grp_id = 0;
    uint8  parent_level = 0;
    uint32 parent_grp_vec_index = 0;
    sys_sch_group_child_node_t *p_sch_group_child = NULL;
    sys_sch_group_node_t *p_sch_group_node = NULL;
    ctc_slistnode_t* ctc_slistnode = NULL;
    uint32 table_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0Config_t, DsQMgrL1Config_t, DsQMgrL2Config_t,
                                                    DsQMgrL3Config_t, DsQMgrGrpConfig_t};
    uint32 flush_field_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0Config_flushEn_f, DsQMgrL1Config_flushEn_f, DsQMgrL2Config_flushEn_f,
                                                    DsQMgrL3Config_flushEn_f, DsQMgrGrpConfig_flushEn_f};
    uint32 pps_field_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0Config_schPpsMode_f, DsQMgrL1Config_schPpsMode_f, DsQMgrL2Config_schPpsMode_f,
                                                    DsQMgrL3Config_schPpsMode_f, DsQMgrGrpConfig_schPpsMode_f};
    uint32 mode_field_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0Config_schModeNode_f, DsQMgrL1Config_schModeNode_f, DsQMgrL2Config_schModeNode_f,
                                                         DsQMgrL3Config_schModeNode_f, DsQMgrGrpConfig_schModeNode_f};

    SYS_LVL_GRP_TO_SUB_GRP(group_id, parent_level, sub_grp_id);

    if(parent_level == skip_level)
    {
        return CTC_E_NONE;
    }
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(parent_grp_vec_index, parent_level, sub_grp_id);
    p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, parent_grp_vec_index);
    if (NULL == p_sch_group_node)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }

    /*set flush*/
    field_val = enable ? 1 : 0;
    if (CTC_IS_BIT_SET(flag, SYS_QOS_SCH_FLAG_FLUSH_EN))
    {
        cmd = DRV_IOW(table_id[parent_level], flush_field_id[parent_level]);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_grp_id, cmd, &field_val));
    }

    if (CTC_IS_BIT_SET(flag, SYS_QOS_SCH_FLAG_WRR_EN))
    {
        cmd = DRV_IOW(table_id[parent_level], pps_field_id[parent_level]);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_grp_id, cmd, &field_val));
        CTC_ERROR_RETURN(_sys_tmm_sch_clear_deficit(lchip, parent_level, sub_grp_id));
    }

    if (CTC_IS_BIT_SET(flag, SYS_QOS_SCH_FLAG_SP_DIS))
    {
        cmd = DRV_IOW(table_id[parent_level], mode_field_id[parent_level]);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_grp_id, cmd, &field_val));
    }

    if(skip_level <= CTC_QOS_SCHED_GROUP_LVL_MAX)
    {
        CTC_SLIST_LOOP(p_sch_group_node->p_child_list, ctc_slistnode)
        {
            p_sch_group_child = _ctc_container_of(ctc_slistnode, sys_sch_group_child_node_t, head);
            CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, p_sch_group_child->child_id, flag, skip_level, enable));
        }
    }
    return ret;
}

int32
_sys_tmm_sch_dump_sched_group_child_list(uint8 lchip, uint32 group_id, uint8 level)
{
    int32  ret = CTC_E_NONE;
    uint32 sub_grp_id = 0;
    uint8  parent_level = 0;
    uint8  child_level = 0;
    uint32 parent_grp_vec_index = 0;
    sys_sch_group_child_node_t *p_sch_group_child = NULL;
    sys_sch_group_node_t *p_sch_group_node = NULL;
    ctc_slistnode_t* ctc_slistnode = NULL;

    SYS_LVL_GRP_TO_SUB_GRP(group_id, parent_level, sub_grp_id);
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(parent_grp_vec_index, parent_level, sub_grp_id);
    p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, parent_grp_vec_index);
    if (NULL == p_sch_group_node)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_DUMP, "|--");
    if(p_sch_group_node->chan_valid)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_DUMP,  "Group-Id:0x%x, Level:%d, chan:%d\n", group_id, parent_level, p_sch_group_node->chan_id);
    }
    else
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_DUMP,  "Group-Id:0x%x, Level:%d, chan:%s\n", group_id, parent_level, "-");
    }
    CTC_SLIST_LOOP(p_sch_group_node->p_child_list, ctc_slistnode)
    {
        p_sch_group_child = _ctc_container_of(ctc_slistnode, sys_sch_group_child_node_t, head);

        SYS_LVL_GRP_TO_SUB_GRP(p_sch_group_child->child_id, child_level, sub_grp_id);
        while (child_level != level)
        {
            SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_DUMP, "|  ");
            child_level--;
        }

        _sys_tmm_sch_dump_sched_group_child_list(lchip, p_sch_group_child->child_id, level);
    }
    return ret;
}

int32
sys_tmm_sch_dump_sched_group_child_list(uint8 lchip, uint32 group_id)
{
    uint32 sub_grp_id = 0;
    uint8  level = 0;
    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);
    return _sys_tmm_sch_dump_sched_group_child_list(lchip, group_id, level);
}

int32
_sys_tmm_sch_destroy_sched_group_recur(uint8 lchip, uint32 group_id)
{
    int32 ret = CTC_E_NONE;
    uint32 sub_grp_id = 0;
    uint8  level = 0;
    uint32 grp_vec_index = 0;
    sys_usw_opf_t opf;
    sys_sch_group_node_t *p_sch_group_node = NULL;
    sys_sch_group_child_node_t *p_sch_group_child = NULL;
    ctc_slistnode_t* ctc_slistnode = NULL;
    ctc_slistnode_t* next = NULL;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&opf, 0, sizeof(sys_usw_opf_t));

    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    if(CTC_QOS_SCHED_GROUP_LVL_Q != level)
    {
        opf.pool_type = p_usw_queue_master[lchip]->opf_type_sch_group;
        opf.pool_index = level;
        sys_usw_opf_free_offset(lchip, &opf, 1, sub_grp_id);
    }

    SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, level, sub_grp_id);

    p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
    if (NULL == p_sch_group_node)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }

    CTC_SLIST_LOOP_DEL(p_sch_group_node->p_child_list, ctc_slistnode, next)
    {
        p_sch_group_child = _ctc_container_of(ctc_slistnode, sys_sch_group_child_node_t, head);
        _sys_tmm_sch_destroy_sched_group_recur(lchip, p_sch_group_child->child_id);
    }

    if(p_sch_group_node->p_child_list)
    {
        ctc_slist_free(p_sch_group_node->p_child_list);
        p_sch_group_node->p_child_list = NULL;
    }

    if(p_sch_group_node->parent_id && level != CTC_QOS_SCHED_GROUP_LVL_R)
    {
        _sys_tmm_sch_unbind_sched_group(lchip, group_id, p_sch_group_node->parent_id);
    }
    ctc_vector_del(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "ctc_vector_del:%d \n", grp_vec_index);
    mem_free(p_sch_group_node);
    return ret;
}

int32
sys_tmm_sch_destroy_sched_group(uint8 lchip, void* p_sched_grp)
{
    ctc_qos_sched_group_id_t* p_sched_group = (ctc_qos_sched_group_id_t*)p_sched_grp;
    uint16 sub_grp_id = 0;
    uint8 level = 0;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    CTC_PTR_VALID_CHECK(p_sched_group);
    SYS_DMA_SCH_GROUP_CHECK(p_sched_group->group_id);
    SYS_LVL_GRP_TO_SUB_GRP(p_sched_group->group_id, level, sub_grp_id);
    SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);

    CTC_ERROR_RETURN(_sys_tmm_sch_destroy_sched_group_recur(lchip, p_sched_group->group_id));

    return CTC_E_NONE;
}


int32
_sys_tmm_sch_unbind_sched_group(uint8 lchip, uint32 child_id, uint32 parent_id)
{
    int32  ret = CTC_E_NONE;
    uint32 parent_sub_grp_id = 0;
    uint8  parent_level = 0;
    uint32 parent_grp_vec_index = 0;
    bool   b_found = FALSE;
    sys_sch_group_child_node_t* p_sch_group_child = NULL;
    sys_sch_group_node_t *p_sch_group_node = NULL;
    ctc_slistnode_t* node = NULL;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    SYS_LVL_GRP_TO_SUB_GRP(parent_id, parent_level, parent_sub_grp_id);
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(parent_grp_vec_index, parent_level, parent_sub_grp_id);
    SYS_USW_REGISTER_WB_SYNC_EN(lchip, CTC_FEATURE_QOS, SYS_WB_APPID_QOS_SUBID_QUEUE_SCH_GROUP_NODE, 1);
    p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, parent_grp_vec_index);
    if (NULL == p_sch_group_node)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }

    CTC_SLIST_LOOP(p_sch_group_node->p_child_list, node)
    {
        p_sch_group_child = _ctc_container_of(node, sys_sch_group_child_node_t, head);
        if (p_sch_group_child->child_id == child_id)
        {
            b_found = TRUE;
            break;
        }
    }

    if (b_found)
    {
        ctc_slist_delete_node(p_sch_group_node->p_child_list, &p_sch_group_child->head);
        mem_free(p_sch_group_child);
    }
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "unbind child id 0x%x to parent id = 0x%x\n", child_id, parent_id);
#if 0
    cmd = DRV_IOR(table_id[child_level], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, child_sub_grp_id, cmd, &parent_select));
    SetDsQMgrGrpParentSelect(V, parentId_f, &parent_select, 0);
    cmd = DRV_IOW(table_id[child_level], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, child_sub_grp_id, cmd, &parent_select));
#endif
    return ret;
}

int32
_sys_tmm_sch_bind_sched_group(uint8 lchip, uint32 child_id, uint32 parent_id, uint8 is_init)
{
    int32  ret = CTC_E_NONE;
    uint32 cmd = 0;
    uint32 tbl_index = 0;
    uint32 child_sub_grp_id = 0;
    uint32 parent_sub_grp_id = 0;
    uint8  child_level = 0;
    uint8  parent_level = 0;
    uint32 child_grp_vec_index = 0;
    uint32 parent_grp_vec_index = 0;
    uint32 old_parent_id = 0;
    sys_sch_group_child_node_t *p_sch_group_child = NULL;
    sys_sch_group_node_t *p_parent_sch_group_node = NULL;
    sys_sch_group_node_t *p_child_sch_group_node = NULL;
    DsQMgrGrpParentSelect_m parent_select;

    uint32 table_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0ParentSelect_t, DsQMgrL1ParentSelect_t, DsQMgrL2ParentSelect_t,
                                                  DsQMgrL3ParentSelect_t, DsQMgrGrpParentSelect_t};

    sal_memset(&parent_select, 0, sizeof(DsQMgrGrpParentSelect_m));

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    SYS_LVL_GRP_TO_SUB_GRP(child_id, child_level, child_sub_grp_id);
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(child_grp_vec_index, child_level, child_sub_grp_id);
    SYS_CHECK_SUB_GROUP_ID(child_sub_grp_id, child_level);

    /*1.Destroy group if parent_id == 0*/
    if (!parent_id)
    {
        return _sys_tmm_sch_destroy_sched_group_recur(lchip, child_id);
    }

    SYS_LVL_GRP_TO_SUB_GRP(parent_id, parent_level, parent_sub_grp_id);
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(parent_grp_vec_index, parent_level, parent_sub_grp_id);
    SYS_CHECK_SUB_GROUP_ID(parent_sub_grp_id, parent_level);

    if ((DRV_IS_TMG(lchip) && (CTC_QOS_SCHED_GROUP_LVL_Q != child_level || CTC_QOS_SCHED_GROUP_LVL_R != parent_level))
     ||(!DRV_IS_TMG(lchip) && (child_level - parent_level != 1)))
    {
        return CTC_E_INVALID_PARAM;
    }

    /*2.check child/parent node */
    p_child_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, child_grp_vec_index);
    if (NULL == p_child_sch_group_node)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }

    p_parent_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, parent_grp_vec_index);
    if (NULL == p_parent_sch_group_node)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }

    if((p_parent_sch_group_node->chan_id == MCHIP_CAP(SYS_CAP_CHANID_DMA_RX0)
        || p_child_sch_group_node->chan_id == MCHIP_CAP(SYS_CAP_CHANID_DMA_RX0))
        && !is_init)
    {
        return CTC_E_INVALID_PARAM;
    }

    /*3.add/update child node*/
    old_parent_id = p_child_sch_group_node->parent_id;
    if (old_parent_id == parent_id)
    {
        return CTC_E_NONE;
    }

    CTC_ERROR_RETURN(_sys_tmm_sch_set_group_chan(lchip, p_child_sch_group_node, p_parent_sch_group_node));

    p_child_sch_group_node->parent_id = parent_id;

    /*4.add child list for parent node*/
    p_sch_group_child = mem_malloc(MEM_QUEUE_MODULE, sizeof(sys_sch_group_child_node_t));
    if (NULL == p_sch_group_child)
    {
        return CTC_E_NO_MEMORY;
    }
    sal_memset(p_sch_group_child, 0, sizeof(sys_sch_group_child_node_t));
    p_sch_group_child->child_id = child_id;

    ctc_slist_add_tail(p_parent_sch_group_node->p_child_list, &p_sch_group_child->head);
    SYS_USW_REGISTER_WB_SYNC_EN(lchip, CTC_FEATURE_QOS, SYS_WB_APPID_QOS_SUBID_QUEUE_SCH_GROUP_NODE, 1);
    /*5.bind child node to parent node*/
    tbl_index = child_sub_grp_id;
    cmd = DRV_IOR(table_id[child_level], DRV_ENTRY_FLAG);
    CTC_ERROR_GOTO(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select), ret, error0);
    SetDsQMgrGrpParentSelect(V, parentId_f, &parent_select, parent_sub_grp_id);
    cmd = DRV_IOW(table_id[child_level], DRV_ENTRY_FLAG);
    CTC_ERROR_GOTO(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select), ret, error0);

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "bind child id 0x%x to parent id = 0x%x\n", child_id, parent_id);

    /*6.unbind old parent node*/
    SYS_LVL_GRP_TO_SUB_GRP(old_parent_id, parent_level, parent_sub_grp_id);
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(parent_grp_vec_index, parent_level, parent_sub_grp_id);
    p_parent_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, parent_grp_vec_index);
    if (p_parent_sch_group_node)
    {
        SYS_USW_REGISTER_WB_SYNC_EN(lchip, CTC_FEATURE_QOS, SYS_WB_APPID_QOS_SUBID_QUEUE_SCH_GROUP_NODE, 1);
        _sys_tmm_sch_unbind_sched_group(lchip, p_sch_group_child->child_id, old_parent_id);
    }

    return CTC_E_NONE;

error0:
    ctc_slist_delete_node(p_parent_sch_group_node->p_child_list, &p_sch_group_child->head);
    mem_free(p_sch_group_child);

    return ret;
}


int32
_sys_tmm_sch_create_sched_group(uint8 lchip, uint32 group_id, uint32 parent_id, uint8 chan_id, uint8 is_init)
{
    int32 ret = CTC_E_NONE;
    uint32 sub_grp_id = 0;
    uint32 grp_vec_index = 0;
    uint8  level = 0;
    uint8  config_flag = 0;
    sys_sch_group_node_t *p_sch_group_node = NULL;

    /*check exist*/
    SYS_LVL_GRP_TO_SUB_GRP(group_id, level, sub_grp_id);
    SYS_SUB_GRP_TO_GRP_VEC_INDEX(grp_vec_index, level, sub_grp_id);
    p_sch_group_node = ctc_vector_get(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
    if (p_sch_group_node)
    {
        return CTC_E_EXIST;
    }

    /*2.Alloc vector node/list for store */
    p_sch_group_node = (sys_sch_group_node_t*)mem_malloc(MEM_QUEUE_MODULE, sizeof(sys_sch_group_node_t));
    if (!p_sch_group_node)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " No memory \n");
        ret = CTC_E_NO_MEMORY;
        goto error0;
    }
    sal_memset(p_sch_group_node, 0, sizeof(sys_sch_group_node_t));

    p_sch_group_node->p_child_list = ctc_slist_new();
    if (p_sch_group_node->p_child_list == NULL)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " No memory \n");
        ret = CTC_E_NO_MEMORY;
        goto error1;
    }

    if(level == CTC_QOS_SCHED_GROUP_LVL_Q || level == CTC_QOS_SCHED_GROUP_LVL_R)
    {
        p_sch_group_node->chan_valid = 1;
        p_sch_group_node->chan_id = chan_id;
        p_sch_group_node->parent_id = level == CTC_QOS_SCHED_GROUP_LVL_R ? chan_id : 0;
    }
    p_sch_group_node->grp_index = grp_vec_index;
    if (!ctc_vector_add(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index, p_sch_group_node))
    {
        ret = CTC_E_NO_MEMORY;
        goto error2;
    }
    SYS_USW_REGISTER_WB_SYNC_EN(lchip, CTC_FEATURE_QOS, SYS_WB_APPID_QOS_SUBID_QUEUE_SCH_GROUP_NODE, 1);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "create sch group id = 0x%x\n", group_id);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "ctc_vector_add:%d \n", grp_vec_index);

    CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_FLUSH_EN);
    CTC_ERROR_GOTO(_sys_tmm_sch_set_group_config(lchip, group_id, config_flag, CTC_QOS_SCHED_GROUP_LVL_MAX, FALSE), ret, error3);

    /*3.Bind parent group id */
    if (parent_id)
    {
        CTC_ERROR_GOTO(_sys_tmm_sch_bind_sched_group(lchip, group_id, parent_id, is_init),
                                                          ret, error3);
    }

    return CTC_E_NONE;

error3:
    ctc_vector_del(p_usw_queue_master[lchip]->sch_group_vec, grp_vec_index);
error2:
    ctc_slist_free(p_sch_group_node->p_child_list);
error1:
    mem_free(p_sch_group_node);
error0:
    return ret;
}
int32
sys_tmm_sch_create_sched_group(uint8 lchip, void* p_sched_grp, uint32 chan)
{
    uint32 cmd = 0;
    int32 ret = CTC_E_NONE;
    uint32 sub_grp_id = 0;
    uint32 chan_id = 0;
    uint32 field_val = 0;
    uint32 group_id_l0 = 0;
    uint8 is_bind = 0;
    uint8 gchip = 0;
    sys_usw_opf_t opf;
    sys_usw_dmps_port_info_t dmps_port_info = {0};
    DsQMgrL0ParentSelect_m  L0_grp_parent;
    ctc_qos_sched_group_id_t* p_sched_group = (ctc_qos_sched_group_id_t*)p_sched_grp;

    CTC_PTR_VALID_CHECK(p_sched_group);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    if (DRV_IS_TMG(lchip) && CTC_QOS_SCHED_GROUP_LVL_Q != p_sched_group->level
                          && CTC_QOS_SCHED_GROUP_LVL_R != p_sched_group->level)
    {
        return CTC_E_NOT_SUPPORT;
    }

    sal_memset(&L0_grp_parent, 0, sizeof(DsQMgrL0ParentSelect_m));
    sal_memset(&opf, 0, sizeof(sys_usw_opf_t));

    /*1.alloc per level group id*/
    if (CTC_QOS_SCHED_GROUP_LVL_Q == p_sched_group->level)
    {
        uint16 queue_id = 0;
        CTC_ERROR_RETURN(sys_usw_queue_get_queue_id(lchip, &(p_sched_group->queue), &queue_id));
        SYS_TMM_GROUP_ID(sub_grp_id, queue_id, 1);
        cmd = DRV_IOR(DsQMgrGrpParentSelect_t, DsQMgrGrpParentSelect_linkToPortEn_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_grp_id, cmd, &field_val));
        if(field_val)
        {
            return CTC_E_INVALID_PARAM;
        }
        if (SYS_COMMON_USELESS_CHANNEL == chan)
        {
            gchip = SYS_MAP_CTC_GPORT_TO_GCHIP(p_sched_group->queue.gport);
            if (FALSE == sys_usw_chip_is_local(lchip, gchip))
            {
                return CTC_E_NONE;
            }
            /* notice gport is physical port can't get channel by SYS_GET_CHANNEL_ID api */
            dmps_port_info.gport = p_sched_group->queue.gport;
            CTC_ERROR_RETURN(sys_usw_port_api_get_dmps_property(lchip, &dmps_port_info, \
                                SYS_PORT_API_DMPS_PROP_PHY_PORT_CHAN_ID, (void*)&chan_id));
        }
        else
        {
            chan_id = chan;
        }
        SYS_SUB_GRP_TO_LVL_GRP(p_sched_group->group_id, CTC_QOS_SCHED_GROUP_LVL_Q, lchip, sub_grp_id);
    }
    else
    {
        if (CTC_QOS_SCHED_GROUP_LVL_R == p_sched_group->level)
        {
            if (SYS_COMMON_USELESS_CHANNEL == chan)
            {
                gchip = SYS_MAP_CTC_GPORT_TO_GCHIP(p_sched_group->gport);
                if (FALSE == sys_usw_chip_is_local(lchip, gchip))
                {
                    return CTC_E_NONE;
                }
                /* notice gport is physical port can't get channel by SYS_GET_CHANNEL_ID api */
                dmps_port_info.gport = p_sched_group->gport;
                CTC_ERROR_RETURN(sys_usw_port_api_get_dmps_property(lchip, &dmps_port_info, \
                                    SYS_PORT_API_DMPS_PROP_PHY_PORT_CHAN_ID, (void*)&chan_id));
            }
            else
            {
                chan_id = chan;
            }
            if (SYS_COMMON_USELESS_CHANNEL == chan_id)
            {
                SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Invalid localphy port \n");
                return CTC_E_INVALID_PORT;
            }

            CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, chan_id, &is_bind, &group_id_l0));
            if (is_bind)
            {
                SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " port has bind L0 \n");
                return CTC_E_EXIST;
            }
        }

        opf.pool_type = p_usw_queue_master[lchip]->opf_type_sch_group;
        opf.pool_index = p_sched_group->level;
        CTC_ERROR_RETURN(sys_usw_opf_alloc_offset(lchip, &opf, 1, &sub_grp_id));
        SYS_SUB_GRP_TO_LVL_GRP(p_sched_group->group_id, p_sched_group->level, lchip, sub_grp_id);

        if (CTC_QOS_SCHED_GROUP_LVL_R == p_sched_group->level)
        {
            cmd = DRV_IOR(DsQMgrL0ParentSelect_t, DRV_ENTRY_FLAG);
            CTC_ERROR_GOTO(DRV_IOCTL(lchip, sub_grp_id, cmd, &L0_grp_parent),ret,error0);
            SetDsQMgrL0ParentSelect(V, parentId_f, &L0_grp_parent, chan_id);
            cmd = DRV_IOW(DsQMgrL0ParentSelect_t, DRV_ENTRY_FLAG);
            CTC_ERROR_GOTO(DRV_IOCTL(lchip, sub_grp_id, cmd, &L0_grp_parent),ret,error0);

            cmd = DRV_IOW(DsQMgrPortConfig_t, DsQMgrPortConfig_l0SchId_f);
            CTC_ERROR_GOTO(DRV_FIELD_IOCTL(lchip, chan_id, cmd, &sub_grp_id),ret,error0);
        }
    }

    CTC_ERROR_GOTO(_sys_tmm_sch_create_sched_group(lchip, p_sched_group->group_id, p_sched_group->parent_id, chan_id, 0),ret,error0);

    return CTC_E_NONE;

error0:
    if(CTC_QOS_SCHED_GROUP_LVL_Q != p_sched_group->level)
    {
        sys_usw_opf_free_offset(lchip, &opf, 1, sub_grp_id);
    }
    return ret;
}
#if 0
int32
sys_tmm_sch_set_span_group_sched(uint8 lchip, uint32 gport, uint8 enable)
{
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM, "gport = %d\n",gport);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM, "enable = %d\n",enable);

    CTC_ERROR_RETURN(_sys_tmm_sch_set_5lvl_group_sched(lchip, gport, MCHIP_CAP(SYS_CAP_QOS_EXT_QUEUE_GRP_NUM) - 1, enable));

    return CTC_E_NONE;
}
#endif
int32
sys_tmm_sch_set_hsched(uint8 lchip, uint32 gport, uint16 group_id_l4, uint8 enable, uint8 type)
{
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM, "gport = %d\n",gport);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM, "group_id = %d\n",group_id_l4);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM, "enable = %d\n",enable);

    if(p_usw_queue_master[lchip]->queue_num_per_stk_port == 1 && type == SYS_EXTEND_QUE_TYPE_C2C)
    {
       return CTC_E_NONE;
    }
    CTC_ERROR_RETURN(_sys_tmm_sch_set_hsched(lchip, gport, group_id_l4, enable, type));

    return CTC_E_NONE;
}

int32
sys_tmm_sch_update_fc_group(uint8 lchip, uint16 chan_id, uint16 ets_group_id, uint8 is_ets, uint8 enable)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint16 loop = 0;
    uint16 ext_group_en = 0;
    DsQMgrGrpFlowCtlMap_m grp_fc_map;
    DsQMgrGrpConfig_m grp_cfg;

    if(!is_ets)
    {
        cmd = DRV_IOR(DsQMgrPortConfig_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &grp_cfg));
        field_val = GetDsQMgrPortConfig(V, groupSchId_f, &grp_cfg);

        cmd = DRV_IOR(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, field_val, cmd, &grp_cfg));
        SetDsQMgrGrpConfig(V, flowCtlEn_f, &grp_cfg, enable ? 1 : 0);
        cmd = DRV_IOW(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, field_val, cmd, &grp_cfg));

        ets_group_id = p_usw_queue_master[lchip]->group_id_ets[chan_id].group_id;
        ext_group_en = ets_group_id != INVALID_QUEUE_GROUP_ID_ETS ? 1 : 0;
    }

    if(is_ets || ext_group_en)
    {
        for (loop = 0; loop < SYS_SCHED_MAX_ETS_EXT_GRP_NUM; loop++)
        {
            if(enable)
            {
                cmd = DRV_IOR(DsQMgrGrpFlowCtlMap_t, DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, ets_group_id + loop, cmd, &grp_fc_map));
                SetDsQMgrGrpFlowCtlMap(V, channelId_f, &grp_fc_map, chan_id);
                cmd = DRV_IOW(DsQMgrGrpFlowCtlMap_t, DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, ets_group_id + loop, cmd, &grp_fc_map));
        
                cmd = DRV_IOR(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, ets_group_id + loop, cmd, &grp_cfg));
                SetDsQMgrGrpConfig(V, flowCtlEn_f, &grp_cfg, 1);
                cmd = DRV_IOW(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, ets_group_id + loop, cmd, &grp_cfg));
            }
            else
            {
                cmd = DRV_IOR(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, ets_group_id + loop, cmd, &grp_cfg));
                SetDsQMgrGrpConfig(V, flowCtlEn_f, &grp_cfg, 0);
                cmd = DRV_IOW(DsQMgrGrpConfig_t, DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, ets_group_id + loop, cmd, &grp_cfg));
            
                cmd = DRV_IOR(DsQMgrGrpFlowCtlMap_t, DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, ets_group_id + loop, cmd, &grp_fc_map));
                SetDsQMgrGrpFlowCtlMap(V, channelId_f, &grp_fc_map, 0);
                cmd = DRV_IOW(DsQMgrGrpFlowCtlMap_t, DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, ets_group_id + loop, cmd, &grp_fc_map));
            }
        }
    }
    return CTC_E_NONE;
}

int32
sys_tmm_sch_update_group_config(uint8 lchip, uint8 first_add, uint8 chan_id, uint16 ets_group_id, ctc_qos_sched_group_t* p_sched)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint16 index = 0;
    uint16 index2 = 0;
    uint16 queue_base1 = 0;
    uint16 queue_base2 = 0;
    ctc_qos_sched_group_id_t sch_group;
    ctc_port_fc_prop_t fc_prop;
    sys_stats_queue_t stats;
    DsErmChannel_m erm_chan;

    sal_memset(&sch_group, 0, sizeof(ctc_qos_sched_group_id_t));
    sal_memset(&fc_prop, 0, sizeof(fc_prop));
    sal_memset(&stats, 0, sizeof(stats));
    if(first_add)
    {
        /*1.update flow control*/
        for (index = 0; index < SYS_PRIO_CLASS_NUM; index++)
        {
            fc_prop.gport = p_sched->queue.gport;
            fc_prop.dir = CTC_INGRESS;
            fc_prop.is_pfc = TRUE;
            fc_prop.priority_class = index;
            CTC_ERROR_RETURN(sys_usw_port_get_flow_ctl_en(lchip, (void*)&fc_prop));
            if(fc_prop.enable)
            {
                CTC_ERROR_RETURN(sys_tmm_sch_update_fc_group(lchip, chan_id, ets_group_id, 1, 1));
            }
        }

        /*2.config ets erm profile and queue base*/
        cmd = DRV_IOR(DsErmChannel_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &erm_chan));
        SetDsErmChannel(V, queueBase_f, &erm_chan, ets_group_id);
        SetDsErmChannel(V, ermProfId_f, &erm_chan, SYS_ERM_PROFILE_ETS);
        cmd = DRV_IOW(DsErmChannel_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &erm_chan)); 

        /*3. sync queue stats, see sys_usw_queue_stats_query process*/
    }
    else
    {
        /*1. all queue_class link to port,delete all*/
        if (p_usw_queue_master[lchip]->group_id_ets[chan_id].group_id == INVALID_QUEUE_GROUP_ID_ETS)
        {
            return CTC_E_NONE;
        }
        cmd = DRV_IOR(DsQMgrPortConfig_t, DsQMgrPortConfig_l0SchId_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, chan_id, cmd, &field_val));
        SYS_SUB_GRP_TO_LVL_GRP(sch_group.group_id , CTC_QOS_SCHED_GROUP_LVL_R, lchip, field_val);
        sys_tmm_sch_destroy_sched_group(lchip, &sch_group);
        _sys_usw_queue_free_ext_group(lchip, SYS_SCHED_MAX_ETS_EXT_GRP_NUM, p_usw_queue_master[lchip]->group_id_ets[chan_id].group_id);
        p_usw_queue_master[lchip]->group_id_ets[chan_id].group_id = INVALID_QUEUE_GROUP_ID_ETS;
        SYS_USW_REGISTER_WB_SYNC_EN(lchip, CTC_FEATURE_QOS, SYS_WB_APPID_QOS_SUBID_QUEUE_MASTER, 1);


        /*2. config ets erm profile and queue base*/
        cmd = DRV_IOR(DsErmChannel_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &erm_chan));
        SetDsErmChannel(V, queueBase_f, &erm_chan, SYS_GROUP_ID_BY_CHAN(chan_id));
        SetDsErmChannel(V, ermProfId_f, &erm_chan, SYS_ERM_PROFILE_DEFAULT);
        cmd = DRV_IOW(DsErmChannel_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &erm_chan));

        /*3. sync queue stats*/      
        for (index = 0; index < MCHIP_CAP(SYS_CAP_QOS_NORMAL_QUEUE_NUM); index++)
        {
            SYS_TMM_QUE_ID_FR_GROUP(queue_base1, SYS_GROUP_ID_BY_CHAN(chan_id), 1);
            for (index2 = 0; index2 < SYS_SCHED_MAX_ETS_EXT_GRP_NUM; index2++)
            {
                SYS_TMM_QUE_ID_FR_GROUP(queue_base2, (ets_group_id + index2), 1);
                CTC_ERROR_RETURN(sys_usw_flow_stats_sync_queue_stats(lchip, (queue_base1+index), (queue_base2+index)));
            }
        }

        /*3. update flow control*/
        for (index = 0; index < SYS_PRIO_CLASS_NUM; index++)
        {
            fc_prop.gport = p_sched->queue.gport;
            fc_prop.dir = CTC_INGRESS;
            fc_prop.is_pfc = TRUE;
            fc_prop.priority_class = index;
            CTC_ERROR_RETURN(sys_usw_port_get_flow_ctl_en(lchip, (void*)&fc_prop));
            if(fc_prop.enable)
            {
                CTC_ERROR_RETURN(sys_tmm_sch_update_fc_group(lchip, chan_id, ets_group_id, 1, 0));
            }
        }
    }
    return CTC_E_NONE;
}

int32
_sys_tmm_sch_set_sub_group_id(uint8 lchip, ctc_qos_sched_group_t* p_sched)
{
    int32 ret = CTC_E_NONE;
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint16 chan_id = 0;
    uint8 is_bind = 0;
    uint8 step = 0;
    uint8 que_map_type = 0;
    uint8 priority = 0;
    uint8 first_add = 0;
    uint8 is_destroy = 0;
    uint16 index = 0;
    uint32 q_grp_id = 0;
    uint32 sch_grp_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {0};
    ctc_qos_sched_group_id_t sch_group;
    sys_sub_group_id_t* sub_group_id_ets = NULL;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    sal_memset(&sch_group, 0, sizeof(ctc_qos_sched_group_id_t));
    /*check*/
    if (p_sched->queue.queue_type != CTC_QUEUE_TYPE_NETWORK_EGRESS)
    {
        return CTC_E_INVALID_PARAM;
    }
    CTC_ERROR_RETURN(_sys_usw_get_channel_by_port(lchip, p_sched->queue.gport, &chan_id));
    CTC_MAX_VALUE_CHECK(p_sched->sub_group_id, 2);
    CTC_MAX_VALUE_CHECK(p_sched->queue_class, QOS_SCHED_MAX_QUE_CLASS);
    sub_group_id_ets = &p_usw_queue_master[lchip]->group_id_ets[chan_id];
    CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, chan_id, &is_bind, &sch_grp_id[CTC_QOS_SCHED_GROUP_LVL_R]));
    if (is_bind)
    {
        if (sub_group_id_ets->group_id == INVALID_QUEUE_GROUP_ID_ETS)
        {
            return CTC_E_INVALID_PARAM;
        }
    }
    else if(sub_group_id_ets->group_id == INVALID_QUEUE_GROUP_ID_ETS && p_sched->sub_group_id == 2)
    {
        return CTC_E_NONE;
    }
    else
    {
        first_add = sub_group_id_ets->group_id == INVALID_QUEUE_GROUP_ID_ETS ? 1 : 0;
    }

    if(first_add)
    {
        uint8 parent_level = 0;
        /*create and bind sch group L0->L1->L2->L3*/
        for (index = CTC_QOS_SCHED_GROUP_LVL_R; index < CTC_QOS_SCHED_GROUP_LVL_Q; index++)
        {
            uint16 grp_num = 0;
            SYS_QOS_SCH_LVL_GRP_NUM(index, grp_num);
            if (0 == grp_num)
            {
                continue;
            }
            sch_group.level = index;
            sch_group.gport = p_sched->queue.gport;
            sch_group.parent_id = (index == CTC_QOS_SCHED_GROUP_LVL_R) ? 0 : sch_grp_id[parent_level];
            CTC_ERROR_GOTO(sys_tmm_sch_create_sched_group(lchip, &sch_group, SYS_COMMON_USELESS_CHANNEL), ret, error0);
            sch_grp_id[index] = sch_group.group_id;
            parent_level = index;
        }

        /*alloc 3 L4 sch group*/
        CTC_ERROR_GOTO(_sys_usw_queue_alloc_ext_group(lchip, 3, 4, &q_grp_id), ret, error0);
        if ((q_grp_id + 2) >= MCHIP_CAP(SYS_CAP_QOS_QUEUE_GRP_BASE_16Q))
        {
            ret = CTC_E_NO_RESOURCE;
            goto error1;
        }
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "ETS:queue/L4 group id base = 0x%x\n", q_grp_id);
        /*
        *bind 3 L4 sch group to L3
        *L4_0->L3_node0, L4_1 / L4_2->L3_node1
        */
        for (index = 0; index < SYS_SCHED_MAX_ETS_EXT_GRP_NUM; index++)
        {
            uint8 parent_lvl = DRV_IS_TMG(lchip) ? CTC_QOS_SCHED_GROUP_LVL_R : CTC_QOS_SCHED_GROUP_LVL_3;
            SYS_SUB_GRP_TO_LVL_GRP(sch_grp_id[CTC_QOS_SCHED_GROUP_LVL_Q], CTC_QOS_SCHED_GROUP_LVL_Q, lchip, q_grp_id + index);
            CTC_ERROR_GOTO(_sys_tmm_sch_create_sched_group(lchip, sch_grp_id[CTC_QOS_SCHED_GROUP_LVL_Q], sch_grp_id[parent_lvl], chan_id, 0), ret, error1);

            field_val = index == 2 ? 1 : 0;
            cmd = DRV_IOW(DsQMgrGrpParentSelect_t, DsQMgrGrpParentSelect_parentNode_f);
            CTC_ERROR_GOTO(DRV_FIELD_IOCTL(lchip, q_grp_id + index, cmd, &field_val), ret, error1);
        }

        /*store allocated queue group id*/
        sub_group_id_ets->group_id= q_grp_id;
        SYS_USW_REGISTER_WB_SYNC_EN(lchip, CTC_FEATURE_QOS, SYS_WB_APPID_QOS_SUBID_QUEUE_MASTER, 1);
    }
    else
    {
        q_grp_id = sub_group_id_ets->group_id;
    }
    /*map queue class to ets sub group id*/
    field_val = p_sched->sub_group_id;
    step = (DsErmPrioScTcMap_g3_1_mappedTcGroup_f - DsErmPrioScTcMap_g3_0_mappedTcGroup_f) * SYS_ERM_PROFILE_ETS;
    cmd = DRV_IOW(DsErmPrioScTcMap_t, DsErmPrioScTcMap_g3_0_mappedTcGroup_f + step);
    for (que_map_type = 0; que_map_type < SYS_QUE_MAP_TYPE_MAX_NUM; que_map_type++)
    {
        for (priority = 0; priority <= MCHIP_CAP(SYS_CAP_QOS_CLASS_PRIORITY_MAX); priority++)
        {
            index = que_map_type << 4 | priority;
            if (priority / 2 == p_sched->queue_class)
            {
                CTC_ERROR_GOTO(DRV_FIELD_IOCTL(lchip, index, cmd, &field_val), ret, error1);
            }
        }
        for (index = 0; index < 3; index++)
        {
            CTC_ERROR_GOTO(_sys_tmm_queue_set_defult_profile(lchip, sub_group_id_ets->group_id + index), ret, error1);
        }

    }
    if(p_sched->sub_group_id == 2)
    {
        CTC_BIT_SET(sub_group_id_ets->group_bmp, p_sched->queue_class);
    }
    else
    {
        CTC_BIT_UNSET(sub_group_id_ets->group_bmp, p_sched->queue_class);
    }
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_INFO, "ETS:sub_group_id_ets->group_bmp = 0x%x\n", sub_group_id_ets->group_bmp);
    is_destroy = sub_group_id_ets->group_bmp == 0xff ? 1 : 0;
    if(first_add || is_destroy)
    {
        CTC_ERROR_GOTO(sys_tmm_sch_update_group_config(lchip, first_add, chan_id, q_grp_id, p_sched), ret, error1);
    }

    return CTC_E_NONE;

error1:
    if(first_add)
    {
        _sys_usw_queue_free_ext_group(lchip, 3, q_grp_id);
    }
error0:
    if(first_add)
    {
        sch_group.group_id = sch_grp_id[CTC_QOS_SCHED_GROUP_LVL_R];
        sys_tmm_sch_destroy_sched_group(lchip, &sch_group);
    }

    return ret;
}

int32
_sys_tmm_sch_set_sub_group_weight(uint8 lchip, ctc_qos_sched_group_t* p_sched)
{
    uint16 chan_id = 0;
    uint32 cmd = 0;
    uint32 field_val = 0;
    sys_sub_group_id_t* sub_group_id_ets = NULL;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    /*check*/
    CTC_MAX_VALUE_CHECK(p_sched->sub_group_id, 1);
    CTC_MAX_VALUE_CHECK(p_sched->weight, 0x7F);
    if (p_sched->queue.queue_type != CTC_QUEUE_TYPE_NETWORK_EGRESS)
    {
        return CTC_E_INVALID_PARAM;
    }

    CTC_ERROR_RETURN(_sys_usw_get_channel_by_port(lchip, p_sched->queue.gport, &chan_id));
    sub_group_id_ets = &p_usw_queue_master[lchip]->group_id_ets[chan_id];
    if (sub_group_id_ets->group_id == INVALID_QUEUE_GROUP_ID_ETS)
    {
        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
        return CTC_E_NOT_EXIST;
    }
    else
    {
        uint16 tbl_id = DRV_IS_TMG(lchip) ? DsQMgrL0ChildWfqWeight_t : DsQMgrL3ChildWfqWeight_t;
        uint32 fld_id = DRV_IS_TMG(lchip) ? DsQMgrL0ChildWfqWeight_weight_f : DsQMgrL3ChildWfqWeight_weight_f;
        field_val = p_sched->weight;
        cmd = DRV_IOW(tbl_id, fld_id);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_group_id_ets->group_id + p_sched->sub_group_id, cmd, &field_val));
    }

    return CTC_E_NONE;
}

int32
_sys_tmm_sch_set_group_sched_state_check(uint8 lchip, uint8 child_level, uint32 child_sub_grp_id)
{
    uint32 state_tbl_id[CTC_QOS_SCHED_GROUP_LVL_Q] = {DsQMgrL1ReqState_t, DsQMgrL2ReqState_t,
                                                      DsQMgrL3ReqState_t, DsQMgrGrpReqState_t};
    uint32 msg_tbl_id[CTC_QOS_SCHED_GROUP_LVL_Q] = {DsQMgrL0QMsg_t, DsQMgrL1QMsg_t,
                                                    DsQMgrL2QMsg_t, DsQMgrL3QMsg_t};
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint32 old_header_ptr = 0;
    uint32 new_header_ptr = 0;
    uint8  parent_level = DRV_IS_TMG(lchip) ? CTC_QOS_SCHED_GROUP_LVL_R : (child_level - 1);
    ds_t ds;

    /*1.check DsQMgrLxReqState, 0 indicate config success*/
    cmd = DRV_IOR(state_tbl_id[child_level - 1], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, child_sub_grp_id, cmd, ds));
    field_val = GetDsQMgrL0ReqState(V, reqExist_f, &ds);

    if (!field_val)
    {
        return CTC_E_NONE;
    }

    /*2. check headptr whether change or not*/
    /*get old headptr*/
    cmd = DRV_IOR(msg_tbl_id[parent_level], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, child_sub_grp_id, cmd, ds));
    old_header_ptr = GetDsQMgrChanQMsg(V, headPtr_f, &ds);

    sal_task_sleep(1);

    /*get new headptr*/
    cmd = DRV_IOR(msg_tbl_id[parent_level], DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, child_sub_grp_id, cmd, ds));
    new_header_ptr = GetDsQMgrChanQMsg(V, headPtr_f, &ds);

    if (new_header_ptr != old_header_ptr)
    {
        return CTC_E_NONE;
    }

    return CTC_E_NOT_READY;
}

int32
_sys_tmm_sch_set_group_sched_adjust(uint8 lchip, uint8 child_level, uint32 child_sub_grp_id)
{
    int32 ret = CTC_E_NONE;
    uint16 loop= 0;
    uint16  channel =0;
    uint32 group_id = 0;
    uint8  is_bind = 0;
    uint8  config_flag = 0;

    /*1. check state 50 times*/
    for (loop = 0; loop < SYS_QOS_SCH_ADJUST_STATE_CHECK_TIME1; loop++)
    {
        ret = _sys_tmm_sch_set_group_sched_state_check(lchip, child_level, child_sub_grp_id);
        if(ret == CTC_E_NONE)
        {
            return CTC_E_NONE;
        }
    }

    CTC_ERROR_RETURN(_sys_tmm_queue_get_channel_by_group_id(lchip, child_level, child_sub_grp_id, &channel));
    CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, channel, &is_bind, &group_id));
    if(is_bind)
    {
        /*2. change parent node to rr*/
        CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_SP_DIS);
        CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, group_id, config_flag, CTC_QOS_SCHED_GROUP_LVL_MAX, TRUE));

        /*3. check state 500 times*/
        for (loop = 0; loop < SYS_QOS_SCH_ADJUST_STATE_CHECK_TIME2; loop++)
        {
            ret = _sys_tmm_sch_set_group_sched_state_check(lchip, child_level, child_sub_grp_id);
            if(ret == CTC_E_NONE)
            {
                break;
            }
        }

        /*4. reset parent node to sp*/
        CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_SP_DIS);
        CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, group_id, config_flag, CTC_QOS_SCHED_GROUP_LVL_MAX, FALSE));
    }

    return ret;
}

int32
sys_tmm_sch_set_group_sched(uint8 lchip, void* p_sched_grp)
{
    uint32 cmd                     = 0;
    uint32 field_val               = 0;
    uint32 tbl_index               = 0;
    uint32 sub_grp_id              = 0;
    uint8  level                   = 0;
    uint8  config_flag             = 0;
    ctc_qos_sched_group_t* p_sched  = (ctc_qos_sched_group_t *)p_sched_grp;
    DsQMgrGrpParentSelect_m parent_select;
    DsQMgrGrpWfqWeight_m grpnode_weight;

    uint32 class_table_id[CTC_QOS_SCHED_GROUP_LVL_Q]  = {DsQMgrL1ParentSelect_t, DsQMgrL2ParentSelect_t,
                                                       DsQMgrL3ParentSelect_t, DsQMgrGrpParentSelect_t};

    uint32 weight_table_id[CTC_QOS_SCHED_GROUP_LVL_Q] = {DsQMgrL0ChildWfqWeight_t, DsQMgrL1ChildWfqWeight_t,
                                                       DsQMgrL2ChildWfqWeight_t, DsQMgrL3ChildWfqWeight_t};

    uint32 node_table_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0WfqWeight_t, DsQMgrL1WfqWeight_t,
                                                       DsQMgrL2WfqWeight_t, DsQMgrL3WfqWeight_t,DsQMgrGrpWfqWeight_t};
    sal_memset(&parent_select, 0, sizeof(DsQMgrGrpParentSelect_m));

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM,  "p_sched->group_id = 0x%x\n", p_sched->group_id);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM,  "p_sched->parent_id = 0x%x\n", p_sched->parent_id);

    if(p_sched->cfg_type != CTC_QOS_SCHED_CFG_SUB_GROUP_ID && p_sched->cfg_type != CTC_QOS_SCHED_CFG_SUB_GROUP_WEIGHT)
    {
        SYS_LVL_GRP_TO_SUB_GRP(p_sched->group_id, level, sub_grp_id);
        SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);
    }

    switch (p_sched->cfg_type)
    {
        case CTC_QOS_SCHED_CFG_CONFIRM_CLASS:
            {
                CTC_MAX_VALUE_CHECK(p_sched->group_type, 0);
                CTC_MIN_VALUE_CHECK(level, CTC_QOS_SCHED_GROUP_LVL_1);
                CTC_MAX_VALUE_CHECK(p_sched->class_priority, MCHIP_CAP(SYS_CAP_QOS_LX_GRP_NODE_NUM) - 1);
                tbl_index = sub_grp_id;
                field_val = p_sched->class_priority;
                cmd = DRV_IOR(class_table_id[level - 1], DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select));
                SetDsQMgrGrpParentSelect(V, parentNode_f, &parent_select, field_val);
                cmd = DRV_IOW(class_table_id[level - 1], DRV_ENTRY_FLAG);
                CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select));

                CTC_ERROR_RETURN(_sys_tmm_sch_set_group_sched_adjust(lchip, level, sub_grp_id));
            }
            break;

        case CTC_QOS_SCHED_CFG_CONFIRM_WEIGHT:
            {
                CTC_MAX_VALUE_CHECK(p_sched->weight, 0x7F);
                if(p_sched->group_type)
                {
                    field_val = (level == CTC_QOS_SCHED_GROUP_LVL_Q) ? MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NODE_NUM)
                                                                     : MCHIP_CAP(SYS_CAP_QOS_LX_GRP_NODE_NUM);
                    CTC_MAX_VALUE_CHECK(p_sched->class_priority, field_val - 1);
                    CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_SP_DIS);
                    CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, p_sched->group_id, config_flag,
                                     CTC_QOS_SCHED_GROUP_LVL_MAX+1, p_sched->weight ? TRUE : FALSE));

                    cmd = DRV_IOR(node_table_id[level], DRV_ENTRY_FLAG);
                    CTC_ERROR_RETURN(DRV_IOCTL(lchip, sub_grp_id, cmd, &grpnode_weight));
                    SetDsQMgrGrpWfqWeight(V, node_0_weight_f + p_sched->class_priority, &grpnode_weight, p_sched->weight);
                    cmd = DRV_IOW(node_table_id[level], DRV_ENTRY_FLAG);
                    CTC_ERROR_RETURN(DRV_IOCTL(lchip, sub_grp_id, cmd, &grpnode_weight));
                }
                else
                {
                    uint8 parent_lvl = DRV_IS_TMG(lchip) ? CTC_QOS_SCHED_GROUP_LVL_R : (level - 1);
                    CTC_MIN_VALUE_CHECK(p_sched->weight, 1);
                    CTC_MIN_VALUE_CHECK(level, CTC_QOS_SCHED_GROUP_LVL_1);
                    tbl_index = sub_grp_id;
                    field_val = p_sched->weight;
                    cmd = DRV_IOR(weight_table_id[parent_lvl], DRV_ENTRY_FLAG);
                    CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select));
                    SetDsQMgrL0ChildWfqWeight(V, weight_f, &parent_select, field_val);
                    cmd = DRV_IOW(weight_table_id[parent_lvl], DRV_ENTRY_FLAG);
                    CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select));
                }
            }
            break;
        case CTC_QOS_SCHED_CFG_SUB_GROUP_ID:
            {
                CTC_ERROR_RETURN(_sys_tmm_sch_set_sub_group_id(lchip, p_sched));
            }
            break;
        case CTC_QOS_SCHED_CFG_SUB_GROUP_WEIGHT:
            {
                CTC_ERROR_RETURN(_sys_tmm_sch_set_sub_group_weight(lchip, p_sched));
            }
            break;
        case CTC_QOS_SCHED_CFG_GROUP_PARENT_ID:
            {
                CTC_MIN_VALUE_CHECK(level, CTC_QOS_SCHED_GROUP_LVL_1);
                CTC_ERROR_RETURN(_sys_tmm_sch_bind_sched_group(lchip, p_sched->group_id, p_sched->parent_id, 0));
                CTC_ERROR_RETURN(_sys_tmm_sch_set_group_sched_adjust(lchip, level, sub_grp_id));
            }
            break;

        default:
            break;
    }

    return CTC_E_NONE;
}

int32
sys_tmm_sch_get_group_sched(uint8 lchip, void* p_sched_grp)
{
    uint32 cmd = 0;
    uint32 tbl_index = 0;
    uint32 sub_grp_id = 0;
    uint32 dot1ae_en = 0;
    uint16 chan_id = 0;
    uint8  level = 0;
    uint8  grp_class     = 0;
    uint8  grp_weight    = 0;
    uint8  link_to_port  = 0;
    uint8  is_bind       = 0;
    uint8  bpe_en        = 0;
    uint8  ext_que_en    = 0;
    uint8  stacking_queue_en  = 0;
    DsQMgrGrpParentSelect_m parent_select;
    sys_extend_que_node_t port_extender_node;
    sys_extend_que_node_t* p_port_extender_node = NULL;
    ctc_qos_sched_group_t* p_sched  = (ctc_qos_sched_group_t *)p_sched_grp;

    uint32 class_table_id[CTC_QOS_SCHED_GROUP_LVL_Q]  = {DsQMgrL1ParentSelect_t, DsQMgrL2ParentSelect_t,
                                                         DsQMgrL3ParentSelect_t, DsQMgrGrpParentSelect_t};

    uint32 weight_table_id[CTC_QOS_SCHED_GROUP_LVL_Q] = {DsQMgrL0ChildWfqWeight_t, DsQMgrL1ChildWfqWeight_t,
                                                         DsQMgrL2ChildWfqWeight_t, DsQMgrL3ChildWfqWeight_t};
    uint32 node_table_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0WfqWeight_t, DsQMgrL1WfqWeight_t,
                                                       DsQMgrL2WfqWeight_t, DsQMgrL3WfqWeight_t,DsQMgrGrpWfqWeight_t};
    sal_memset(&parent_select, 0, sizeof(DsQMgrGrpParentSelect_m));
    sal_memset(&port_extender_node, 0, sizeof(port_extender_node));

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    if (p_sched->cfg_type != CTC_QOS_SCHED_CFG_SUB_GROUP_ID && p_sched->cfg_type != CTC_QOS_SCHED_CFG_SUB_GROUP_WEIGHT)
    {
        if (0 == p_sched->group_id)
        {
            if (SYS_MAP_CTC_GPORT_TO_PORT_TYPE(p_sched->queue.gport) == CTC_GPORT_TYPE_DMA)
            {
                port_extender_node.channel = MCHIP_CAP(SYS_CAP_CHANID_DMA_RX0);
            }
            else
            {
                CTC_ERROR_RETURN(_sys_usw_get_channel_by_port(lchip, p_sched->queue.gport, &chan_id));
                port_extender_node.channel = chan_id;
            }
            _sys_usw_queue_get_ext_queue_enable(lchip, p_sched->queue.gport, &stacking_queue_en, &bpe_en, &dot1ae_en, &ext_que_en);
            if (ext_que_en && !CTC_IS_CPU_PORT(p_sched->queue.gport))
            {
                port_extender_node.type = bpe_en ? SYS_EXTEND_QUE_TYPE_BPE : SYS_EXTEND_QUE_TYPE_C2C;
                port_extender_node.lchip = lchip;
                port_extender_node.lport = bpe_en ? CTC_MAP_GPORT_TO_LPORT(p_sched->queue.gport) : 0;
                p_port_extender_node = sys_usw_port_extender_lookup(lchip, &port_extender_node);
                if(p_port_extender_node)
                {
                    is_bind = 1;
                    SYS_SUB_GRP_TO_LVL_GRP(sub_grp_id , CTC_QOS_SCHED_GROUP_LVL_Q, lchip, p_port_extender_node->group_id);
                }
            }
            else
            {
                CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, port_extender_node.channel, &is_bind, &sub_grp_id));
            }
            if (is_bind && sub_grp_id)
            {
                if (p_sched->child_cnt >= 1)
                {
                    p_sched->child_id_list[0] = sub_grp_id;
                }
                p_sched->child_cnt = 1;
                return CTC_E_NONE;
            }
        }
        SYS_LVL_GRP_TO_SUB_GRP(p_sched->group_id, level, sub_grp_id);
        SYS_CHECK_SUB_GROUP_ID(sub_grp_id, level);

        if (p_sched->group_type)
        {
            grp_class = (level == CTC_QOS_SCHED_GROUP_LVL_Q) ? MCHIP_CAP(SYS_CAP_QOS_QUE_GRP_NODE_NUM) : MCHIP_CAP(SYS_CAP_QOS_LX_GRP_NODE_NUM);
            CTC_MAX_VALUE_CHECK(p_sched->class_priority, grp_class - 1);
            cmd = DRV_IOR(node_table_id[level], DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, sub_grp_id, cmd, &parent_select));
            p_sched->weight = GetDsQMgrGrpWfqWeight(V, node_0_weight_f + p_sched->class_priority, &parent_select);
            CTC_ERROR_RETURN(_sys_tmm_sch_get_sched_group(lchip, p_sched->group_id, &p_sched->parent_id));
        }
        else if (level != CTC_QOS_SCHED_GROUP_LVL_R)
        {
            uint8 parent_lvl = DRV_IS_TMG(lchip) ? CTC_QOS_SCHED_GROUP_LVL_R : (level - 1);
            tbl_index = sub_grp_id;
            cmd = DRV_IOR(class_table_id[level - 1], DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select));
            link_to_port = GetDsQMgrGrpParentSelect(V, linkToPortEn_f, &parent_select);
            grp_class = GetDsQMgrGrpParentSelect(V, parentNode_f, &parent_select);


            tbl_index = sub_grp_id;
            cmd = DRV_IOR(weight_table_id[parent_lvl], DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, tbl_index, cmd, &parent_select));
            grp_weight = GetDsQMgrL0ChildWfqWeight(V, weight_f, &parent_select);

            if (!link_to_port)
            {
                p_sched->class_priority = grp_class;
                p_sched->weight = grp_weight;
                CTC_ERROR_RETURN(_sys_tmm_sch_get_sched_group(lchip, p_sched->group_id, &p_sched->parent_id));
            }
        }

        CTC_ERROR_RETURN(_sys_tmm_sch_get_sched_group_child_list(lchip, p_sched->group_id, p_sched->child_id_list, &p_sched->child_cnt));
    }
    else
    {
        uint8 step = 0;
        uint16 chan_id = 0;
        uint32 field_val = 0;
        sys_sub_group_id_t* sub_group_id_ets = NULL;

        CTC_MAX_VALUE_CHECK(p_sched->queue_class, QOS_SCHED_MAX_QUE_CLASS);
        if (p_sched->queue.queue_type != CTC_QUEUE_TYPE_NETWORK_EGRESS)
        {
            return CTC_E_INVALID_PARAM;
        }
        CTC_ERROR_RETURN(_sys_usw_get_channel_by_port(lchip, p_sched->queue.gport, &chan_id));
        sub_group_id_ets = &p_usw_queue_master[lchip]->group_id_ets[chan_id];
        if (sub_group_id_ets->group_id == INVALID_QUEUE_GROUP_ID_ETS)
        {
            SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Entry not exist \n");
            return CTC_E_NOT_EXIST;
        }
        else
        {
            step = (DsErmPrioScTcMap_g3_1_mappedTcGroup_f - DsErmPrioScTcMap_g3_0_mappedTcGroup_f) * SYS_ERM_PROFILE_ETS;
            cmd = DRV_IOR(DsErmPrioScTcMap_t, DsErmPrioScTcMap_g3_0_mappedTcGroup_f + step);
            CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, p_sched->queue_class * 2, cmd, &field_val));
            p_sched->sub_group_id = field_val;
            if (p_sched->sub_group_id != 2)
            {
                uint16 tbl_id = DRV_IS_TMG(lchip) ? DsQMgrL0ChildWfqWeight_t : DsQMgrL3ChildWfqWeight_t;
                uint32 fld_id = DRV_IS_TMG(lchip) ? DsQMgrL0ChildWfqWeight_weight_f : DsQMgrL3ChildWfqWeight_weight_f;
                cmd = DRV_IOR(tbl_id, fld_id);
                CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_group_id_ets->group_id + p_sched->sub_group_id, cmd, &field_val));
                p_sched->weight = field_val;
            }
        }
    }
    return CTC_E_NONE;
}


int32
sys_tmm_sch_set_queue_sched(uint8 lchip, uint16 queue_id, uint8 type, uint8 queue_offset, uint8 exceed_class, uint16 exceed_weight)
{
    uint32 cmd           = 0;
    uint32 field_val     = 0;
    uint32 field_id      = 0;
    uint32 sub_grp_id    = 0;
    uint32 group_index   = 0;
    uint8  grp_mode      = 1;
    uint32 table_id      = 0;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);

    SYS_TMM_GROUP_ID(sub_grp_id, queue_id, grp_mode);
    SYS_TMM_GROUP_INDEX(group_index, sub_grp_id);

    if (queue_id >= MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_NETWORK_MISC))
    {
        return CTC_E_INVALID_PARAM;
    }

    switch (type)
    {
    case CTC_QOS_SCHED_CFG_CONFIRM_CLASS:
    case CTC_QOS_SCHED_CFG_CONFIRM_WEIGHT:

        SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_ERROR, " Feature not supported \n");
        return CTC_E_NOT_SUPPORT;

    case CTC_QOS_SCHED_CFG_EXCEED_CLASS:

        CTC_MAX_VALUE_CHECK(exceed_class, MCHIP_CAP(SYS_CAP_QOS_SCHED_MAX_QUE_CLASS));
        field_val = exceed_class;
        field_id = DsQMgrQueSchMap_cos_0_nodeSel_f + queue_offset;
        cmd = DRV_IOW(DsQMgrQueSchMap_t, field_id);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_grp_id, cmd, &field_val));
        CTC_ERROR_RETURN(_sys_tmm_sch_clear_deficit(lchip, CTC_QOS_SCHED_GROUP_LVL_Q, sub_grp_id));
        break;
    case CTC_QOS_SCHED_CFG_EXCEED_WEIGHT:

        CTC_MIN_VALUE_CHECK(exceed_weight, 1);
        CTC_MAX_VALUE_CHECK(exceed_weight, 0x7F);

        table_id = (CTC_IS_BIT_SET(queue_id, 3) && !CTC_IS_BIT_SET(sub_grp_id, 10)) ? DsQMgrQueWfqWeight1_t : DsQMgrCosWfqWeight_t;
        field_id = (CTC_IS_BIT_SET(queue_id, 3) && !CTC_IS_BIT_SET(sub_grp_id, 10)) ? (DsQMgrQueWfqWeight1_g_0_weight_f + queue_offset)
                   : (DsQMgrCosWfqWeight_g_0_weight_f + queue_offset);

        field_val = exceed_weight;
        cmd = DRV_IOW(table_id, field_id);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_index, cmd, &field_val));

        break;

    default:
        return CTC_E_INVALID_PARAM;
    }

    return CTC_E_NONE;
}

int32
sys_tmm_sch_get_queue_sched(uint8 lchip, uint16 queue_id, uint8 queue_offset, uint8* exceed_class, uint16* exceed_weight)
{
    uint32 cmd           = 0;
    uint32 field_val     = 0;
    uint32 field_id      = 0;
    uint32 sub_grp_id    = 0;
    uint32 group_index   = 0;
    uint8  grp_mode      = 1;
    uint32 table_id      = 0;
    uint8  que_class     = 0;
    uint16 que_weight    = 0;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    CTC_MAX_VALUE_CHECK(*exceed_class, MCHIP_CAP(SYS_CAP_QOS_SCHED_MAX_QUE_CLASS));

    SYS_TMM_GROUP_ID(sub_grp_id, queue_id, grp_mode);
    SYS_TMM_GROUP_INDEX(group_index, sub_grp_id);

    if (queue_id < MCHIP_CAP(SYS_CAP_QOS_QUEUE_BASE_NETWORK_MISC))
    {

        field_id = DsQMgrQueSchMap_cos_0_nodeSel_f + queue_offset;
        cmd = DRV_IOR(DsQMgrQueSchMap_t, field_id);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_grp_id, cmd, &field_val));
        que_class = field_val;
        *exceed_class = que_class;

        table_id = (CTC_IS_BIT_SET(queue_id, 3) && !CTC_IS_BIT_SET(sub_grp_id, 10)) ? DsQMgrQueWfqWeight1_t : DsQMgrCosWfqWeight_t;
        field_id = (CTC_IS_BIT_SET(queue_id, 3) && !CTC_IS_BIT_SET(sub_grp_id, 10)) ? (DsQMgrQueWfqWeight1_g_0_weight_f + queue_offset)
        : (DsQMgrCosWfqWeight_g_0_weight_f + queue_offset);
        cmd = DRV_IOR(table_id, field_id);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_index, cmd, &field_val));
        que_weight = field_val;
        *exceed_weight = que_weight;
    }
    else
    {
        *exceed_class = queue_offset;
    }

    return CTC_E_NONE;
}

int32
_sys_tmm_sch_bind_sched_group_for_dma_chan(uint8 lchip, uint32 dma_grp_base, uint8 grp_num)
{
    int32  ret = CTC_E_NONE;
    uint8  index = 0;
    uint8  grp_index = 0;
    ctc_qos_sched_group_id_t sch_group;
    uint32 grp_id[CTC_QOS_SCHED_GROUP_LVL_Q] = {0};
    uint32 group_id_lvl_q = 0;
    uint32 sub_grp_id = 0;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);


    for (index = CTC_QOS_SCHED_GROUP_LVL_R; index < CTC_QOS_SCHED_GROUP_LVL_Q; index++)
    {
        uint16 tmp_grp_num = 0;
        SYS_QOS_SCH_LVL_GRP_NUM(index, tmp_grp_num);
        if (0 == tmp_grp_num)
        {
            continue;
        }
        sal_memset(&sch_group, 0, sizeof(ctc_qos_sched_group_id_t));
        sch_group.level = index;
        sch_group.gport = 0x10000000;
        CTC_ERROR_RETURN(sys_tmm_sch_create_sched_group(lchip, &sch_group, MCHIP_CAP(SYS_CAP_CHANID_DMA_RX0)));
        grp_id[index] = sch_group.group_id;
    }

    if (!DRV_IS_TMG(lchip))
    {
        for (index = CTC_QOS_SCHED_GROUP_LVL_R; index < CTC_QOS_SCHED_GROUP_LVL_3; index++)
        {
            CTC_ERROR_GOTO(_sys_tmm_sch_bind_sched_group(lchip, grp_id[index+1], grp_id[index], 1),ret, error0);
        }
    }

    for (grp_index = 0; grp_index < grp_num; grp_index++)
    {
        uint16 parent_lvl = DRV_IS_TMG(lchip) ? CTC_QOS_SCHED_GROUP_LVL_R : CTC_QOS_SCHED_GROUP_LVL_3;
        sub_grp_id = dma_grp_base + grp_index;
        SYS_SUB_GRP_TO_LVL_GRP(group_id_lvl_q, CTC_QOS_SCHED_GROUP_LVL_Q, lchip, sub_grp_id);
        CTC_ERROR_GOTO(_sys_tmm_sch_create_sched_group(lchip, group_id_lvl_q, grp_id[parent_lvl],
                                                        MCHIP_CAP(SYS_CAP_CHANID_DMA_RX0), 1), ret, error0);
    }

    return CTC_E_NONE;

error0:
    sch_group.group_id = grp_id[CTC_QOS_SCHED_GROUP_LVL_R];
    sys_tmm_sch_destroy_sched_group(lchip, &sch_group);

    return ret;
}

/*long pkt change sp to wrr slowly, need clear deficit*/
int32
_sys_tmm_sch_clear_deficit(uint8 lchip, uint8 level, uint32 sub_grp_id)
{
    uint32 cmd = 0;
    uint32 group_index = 0;
    uint32 deficit_table_id[CTC_QOS_SCHED_GROUP_LVL_MAX] = {DsQMgrL0WfqDeficit_t, DsQMgrL1WfqDeficit_t, DsQMgrL2WfqDeficit_t,
                                                    DsQMgrL3WfqDeficit_t, DsQMgrGrpWfqDeficit_t};
    uint8  loop = 0;
    ds_t ds;
    sal_memset(&ds, 0, sizeof(ds));
    do
    {
        cmd = DRV_IOW(deficit_table_id[level], DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, sub_grp_id, cmd, ds));
        if(level == CTC_QOS_SCHED_GROUP_LVL_Q)
        {
            SYS_TMM_GROUP_INDEX(group_index, sub_grp_id);
            cmd = DRV_IOW(((CTC_IS_BIT_SET(sub_grp_id, 0) && !CTC_IS_BIT_SET(sub_grp_id, 10)) ? DsQMgrQueWfqDeficit1_t : DsQMgrCosWfqDeficit_t), DRV_ENTRY_FLAG);
            CTC_ERROR_RETURN(DRV_IOCTL(lchip, group_index, cmd, ds))
        }
        //sal_task_sleep(10);
    }while((loop++) < SYS_QOS_SCH_CLAER_DEFICIT_TIMES);

    return CTC_E_NONE;

}

int32
sys_tmm_sch_wrr_en(uint8 lchip, uint32 gport, uint32* enable, uint8 is_set)
{
    uint32 chan_id = 0;
    uint32 group_id_l0 = 0;
    uint32 group_id_l4 = 0;
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint8  is_bind = 0;
    uint8  config_flag = 0;
    sys_usw_dmps_port_info_t dmps_port_info = {0};

    CTC_PTR_VALID_CHECK(enable);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM,  "gport = %d\n", gport);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM,  "enable = %d\n", *enable);

    field_val = is_set ? *enable : 0;
    dmps_port_info.gport = gport;
    CTC_ERROR_RETURN(sys_usw_port_api_get_dmps_property(lchip, &dmps_port_info, SYS_PORT_API_DMPS_PROP_CHAN_ID, &chan_id));
    CTC_ERROR_RETURN(_sys_tmm_sch_check_chan_bind_root_group(lchip, chan_id, &is_bind, &group_id_l0));

    if (is_bind)
    {
        if (is_set)
        {
            CTC_BIT_SET(config_flag, SYS_QOS_SCH_FLAG_WRR_EN);
            CTC_ERROR_RETURN(_sys_tmm_sch_set_group_config(lchip, group_id_l0, config_flag,
                                                        CTC_QOS_SCHED_GROUP_LVL_MAX, field_val ? TRUE : FALSE));
        }
        else
        {
            uint16 sub_group_id = 0;
            /*group_id_l0 equal to sub_group_id*/
            SYS_LVL_GRP_TO_SUB_GRP(group_id_l0, field_val, sub_group_id);
            cmd = DRV_IOR(DsQMgrL0Config_t, DsQMgrL0Config_schPpsMode_f);
            CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, sub_group_id, cmd, &field_val));
            *enable = field_val;
        }
    }
    else
    {
        cmd = DRV_IOR(DsQMgrPortConfig_t, DsQMgrPortConfig_groupSchId_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, chan_id, cmd, &group_id_l4));

        if (is_set)
        {
            cmd = DRV_IOW(DsQMgrGrpConfig_t, DsQMgrGrpConfig_schPpsMode_f);
            CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_id_l4, cmd, &field_val));
            CTC_ERROR_RETURN(_sys_tmm_sch_clear_deficit(lchip, CTC_QOS_SCHED_GROUP_LVL_Q, group_id_l4));
        }
        else
        {
            cmd = DRV_IOR(DsQMgrGrpConfig_t, DsQMgrGrpConfig_schPpsMode_f);
            CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, group_id_l4, cmd, &field_val));
            *enable = field_val;
        }
    }

    return CTC_E_NONE;
}

extern int32
_sys_usw_queue_sch_free_node_data(void* node_data, void* user_data);

int32
sys_tmm_qos_sch_init(uint8 lchip)
{
    int32 ret = CTC_E_NONE;
    uint8 level = 0;
    uint16 loop = 0;
    uint32 max_size_lvl_q = 0;
    uint32 max_size_lvl_3 = 0;
    uint32 max_size_lvl_2 = 0;
    uint32 max_size_lvl_1 = 0;
    uint32 max_size_lvl_r = 0;
    uint32 max_size       = 0;
    uint32 dma_grp_base   = 0;
    uint8  grp_num_per_dma_chan = 0;
    sys_usw_opf_t opf;
    uint32 sch_grp_cnt[CTC_QOS_SCHED_GROUP_LVL_Q] = {0};

    sys_usw_ftm_query_table_entry_num(lchip, DsQMgrGrpParentSelect_t, &max_size_lvl_q);
    sys_usw_ftm_query_table_entry_num(lchip, DsQMgrL3ParentSelect_t, &max_size_lvl_3);
    sys_usw_ftm_query_table_entry_num(lchip, DsQMgrL2ParentSelect_t, &max_size_lvl_2);
    sys_usw_ftm_query_table_entry_num(lchip, DsQMgrL1ParentSelect_t, &max_size_lvl_1);
    sys_usw_ftm_query_table_entry_num(lchip, DsQMgrL0ParentSelect_t, &max_size_lvl_r);
    sch_grp_cnt[CTC_QOS_SCHED_GROUP_LVL_R] = max_size_lvl_r;
    sch_grp_cnt[CTC_QOS_SCHED_GROUP_LVL_1] = max_size_lvl_1;
    sch_grp_cnt[CTC_QOS_SCHED_GROUP_LVL_2] = max_size_lvl_2;
    sch_grp_cnt[CTC_QOS_SCHED_GROUP_LVL_3] = max_size_lvl_3;
    max_size = max_size_lvl_q + max_size_lvl_3 + max_size_lvl_2 + max_size_lvl_1 + max_size_lvl_r;

    p_usw_queue_master[lchip]->sch_group_vec = ctc_vector_init(32, CTC_VEC_BLOCK_NUM(max_size, 32));
    if (NULL == p_usw_queue_master[lchip]->sch_group_vec)
    {
        ret = CTC_E_NO_MEMORY;
    }

    /*init sch group opf*/
    CTC_ERROR_GOTO(sys_usw_opf_init(lchip, &p_usw_queue_master[lchip]->opf_type_sch_group, 4, "opf-sch-group"), ret, error0);

    opf.pool_type  = p_usw_queue_master[lchip]->opf_type_sch_group;
    for (level = 0; level < CTC_QOS_SCHED_GROUP_LVL_Q; level++)
    {
        opf.pool_index = level;
        CTC_ERROR_GOTO(sys_usw_opf_init_offset(lchip, &opf, 0, sch_grp_cnt[level]), ret, error1);
    }

    grp_num_per_dma_chan = p_usw_queue_master[lchip]->queue_num_for_cpu_reason / MCHIP_CAP(SYS_CAP_REASON_GRP_QUEUE_NUM);
    dma_grp_base = MCHIP_CAP(SYS_CAP_CHANNEL_NUM);
    _sys_tmm_sch_bind_sched_group_for_dma_chan(lchip, dma_grp_base, grp_num_per_dma_chan);

    for(loop = 0; loop < SYS_MAX_CHANNEL_NUM; loop++)
    {
        p_usw_queue_master[lchip]->group_id_ets[loop].group_id = INVALID_QUEUE_GROUP_ID_ETS;
    }

    CTC_ERROR_RETURN(SYS_CB_REGISTER(lchip, SYS_CB_QOS_WRR_EN, sys_tmm_sch_wrr_en));

    return ret;

error1:
    sys_usw_opf_deinit(lchip, p_usw_queue_master[lchip]->opf_type_sch_group);
error0:
    ctc_vector_traverse(p_usw_queue_master[lchip]->sch_group_vec, (vector_traversal_fn)_sys_usw_queue_sch_free_node_data, NULL);
    ctc_vector_release(p_usw_queue_master[lchip]->sch_group_vec);
    return ret;
}

#define __RESRC__

int32
sys_tmm_resrc_set_que_drop(uint8 lchip, uint8 is_dynamic, uint8 enable, void* que_drop_array,
                                                void* p_queue)
{
    uint32 cmd = 0;
    uint8  cng_level = 0;
    uint32 index = 0;
    uint8 drop_factor[11] = {15,14,13,12,11,10,9,0,1,2,3};
    DsErmQueueLimitedThrdProfile_m ds_que_thrd_profile;
    ctc_qos_drop_array drop_array;
    sys_queue_node_t* p_sys_queue = (sys_queue_node_t*)p_queue;

    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    CTC_PTR_VALID_CHECK(p_sys_queue);

    sal_memset(&ds_que_thrd_profile, 0, sizeof(DsErmQueueLimitedThrdProfile_m));
    sal_memset(drop_array, 0, sizeof(ctc_qos_drop_array));
    sal_memcpy(drop_array, que_drop_array, sizeof(ctc_qos_drop_array));

    index = p_sys_queue->queue_id;
    cmd = DRV_IOR(DsErmQueueLimitedThrdProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &ds_que_thrd_profile));
    SetDsErmQueueLimitedThrdProfile(V, g_0_queueLimitedThrd_f, &ds_que_thrd_profile, enable ? drop_array[cng_level].drop.max_th[0] / 8 : 0x3FFF);
    SetDsErmQueueLimitedThrdProfile(V, g_1_queueLimitedThrd_f, &ds_que_thrd_profile, enable ? drop_array[cng_level].drop.max_th[1] / 8 : 0x3FFF);
    SetDsErmQueueLimitedThrdProfile(V, g_2_queueLimitedThrd_f, &ds_que_thrd_profile, enable ? drop_array[cng_level].drop.max_th[2] / 8 : 0x3FFF);
    SetDsErmQueueLimitedThrdProfile(V, g_3_queueLimitedThrd_f, &ds_que_thrd_profile, enable ? drop_array[cng_level].drop.max_th[3] / 8 : 0x3FFF);
    SetDsErmQueueLimitedThrdProfile(V, g_0_ecnMarkThrd_f, &ds_que_thrd_profile, (enable ? drop_array[cng_level].drop.ecn_th[0] : MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD)) / 8);
    SetDsErmQueueLimitedThrdProfile(V, g_1_ecnMarkThrd_f, &ds_que_thrd_profile, (enable ? drop_array[cng_level].drop.ecn_th[1] : MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD)) / 8);
    SetDsErmQueueLimitedThrdProfile(V, g_2_ecnMarkThrd_f, &ds_que_thrd_profile, (enable ? drop_array[cng_level].drop.ecn_th[2] : MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD)) / 8);
    SetDsErmQueueLimitedThrdProfile(V, g_3_ecnMarkThrd_f, &ds_que_thrd_profile, (enable ? drop_array[cng_level].drop.ecn_th[3] : MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD)) / 8);
    SetDsErmQueueLimitedThrdProfile(V, g_0_queueLimitedFactor_f, &ds_que_thrd_profile, drop_factor[drop_array[cng_level].drop.drop_factor[0]]);
    SetDsErmQueueLimitedThrdProfile(V, g_1_queueLimitedFactor_f, &ds_que_thrd_profile, drop_factor[drop_array[cng_level].drop.drop_factor[1]]);
    SetDsErmQueueLimitedThrdProfile(V, g_2_queueLimitedFactor_f, &ds_que_thrd_profile, drop_factor[drop_array[cng_level].drop.drop_factor[2]]);
    SetDsErmQueueLimitedThrdProfile(V, g_3_queueLimitedFactor_f, &ds_que_thrd_profile, drop_factor[drop_array[cng_level].drop.drop_factor[3]]);
    SetDsErmQueueLimitedThrdProfile(V, g_0_queueLimitedAutoEn_f, &ds_que_thrd_profile, is_dynamic ? 1 : 0);
    SetDsErmQueueLimitedThrdProfile(V, g_1_queueLimitedAutoEn_f, &ds_que_thrd_profile, is_dynamic ? 1 : 0);
    SetDsErmQueueLimitedThrdProfile(V, g_2_queueLimitedAutoEn_f, &ds_que_thrd_profile, is_dynamic ? 1 : 0);
    SetDsErmQueueLimitedThrdProfile(V, g_3_queueLimitedAutoEn_f, &ds_que_thrd_profile, is_dynamic ? 1 : 0);
    cmd = DRV_IOW(DsErmQueueLimitedThrdProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &ds_que_thrd_profile));

    return CTC_E_NONE;
}

int32
sys_tmm_resrc_fc_get_profile(uint8 lchip, uint16 chan_id, void* p_resrc_fc)
{
    uint16 index = 0;
    uint32 cmd = 0;
    uint32 value = 0;
    uint8 step = 0;
    ctc_qos_resrc_fc_t *p_fc = (ctc_qos_resrc_fc_t *)p_resrc_fc;
    DsIrmPortFlowControlThrdProfile_m ds_igr_port_fc_profile;

    CTC_PTR_VALID_CHECK(p_fc);

    sal_memset(&ds_igr_port_fc_profile, 0, sizeof(DsIrmPortFlowControlThrdProfile_m));

    index = chan_id;
    cmd = DRV_IOR(DsIrmPortFlowControlThrdProfile_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, index, cmd, &ds_igr_port_fc_profile));

    step = DsIrmPortFlowControlThrdProfile_g_1_xoffThrd_f - DsIrmPortFlowControlThrdProfile_g_0_xoffThrd_f;
    p_fc->priority_class = p_fc->is_pfc ? p_fc->priority_class : 0;
    p_fc->xoff_thrd = GetDsIrmPortFlowControlThrdProfile(V, g_0_xoffThrd_f + p_fc->priority_class*step, &ds_igr_port_fc_profile);
    p_fc->xon_thrd = GetDsIrmPortFlowControlThrdProfile(V, g_0_xonThrd_f + p_fc->priority_class*step, &ds_igr_port_fc_profile);

    if (p_fc->is_pfc)
    {
        cmd = DRV_IOR(DsIrmPortTcLimitedThrdProfile_t, DsIrmPortTcLimitedThrdProfile_g_0_portTcLimitedThrd_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, (index << 3) + p_fc->priority_class, cmd, &value));
    }
    else
    {
        cmd = DRV_IOR(DsIrmPortLimitedThrdProfile_t, DsIrmPortLimitedThrdProfile_g_0_portLimitedThrd_f);
        CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, index, cmd, &value));
    }

    p_fc->drop_thrd = ((value * 8) > MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD)) ? MCHIP_CAP(SYS_CAP_QOS_QUEUE_MAX_DROP_THRD) : (value * 8);

    return CTC_E_NONE;
}

int32
sys_tmm_queue_drop_read_asic(uint8 lchip, uint8 wred_mode, void* p_queue_node, void* p_sys_profile, uint8* hw_valid)
{
    uint32 cmd = 0;
    uint32 field_val = 0;
    uint16 queue_id = ((sys_queue_node_t*)p_queue_node)->queue_id;

    if (!wred_mode)
    {
        return CTC_E_NONE;
    }
    
    cmd = DRV_IOR(DsErmAqmQueueThrdProfileId_t, DsErmAqmQueueThrdProfileId_g_0_aqmThrdProfileId_f + (queue_id&0x3));
    CTC_ERROR_RETURN(DRV_FIELD_IOCTL(lchip, queue_id >> 2, cmd, &field_val));
    ((sys_queue_drop_profile_t*)p_sys_profile)[0].p_drop_wred_profile->profile_id = field_val;
    *hw_valid = 1;
    return CTC_E_NONE;
}

#define __FCDL__

STATIC int32
_sys_tmm_qos_store_buffer_thrd(uint8 lchip, uint16 queue_id, uint32 chan_id, uint8 pfc_en)
{
    sys_qos_buffer_info_tmm_t* p_buffer_info = NULL;
    sys_qos_buffer_info_tmm_t buffer_info_tmp;
    ErmMiscCtl_m erm_misc_ctl;
    uint32 cmd = 0;
    int32 ret = CTC_E_NONE;

    p_buffer_info = ctc_vector_get(p_usw_queue_master[lchip]->buffer_info_vec, chan_id);
    if (!p_buffer_info)
    {
        MALLOC_ZERO(MEM_QUEUE_MODULE, p_buffer_info, sizeof(sys_qos_buffer_info_tmm_t));
        if (!p_buffer_info)
        {
            return CTC_E_NO_MEMORY;
        }
        sal_memset(&buffer_info_tmp, 0, sizeof(sys_qos_buffer_info_tmm_t));
    }
    cmd = DRV_IOR(ErmMiscCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_GOTO(DRV_IOCTL(lchip, 0, cmd, &erm_misc_ctl), ret, error0);
    SetErmMiscCtl(V, guaranteedMode_f, &erm_misc_ctl, 0);/*bit0:porttc 1:portsc 2:port guarantee*/
    SetErmMiscCtl(V, thrdPassMode_f, &erm_misc_ctl, 0);
    cmd = DRV_IOW(ErmMiscCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_GOTO(DRV_IOCTL(lchip, 0, cmd, &erm_misc_ctl), ret, error0);

    if(pfc_en)
    {
        /* queue limited */
        cmd = DRV_IOR(DsErmQueueLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_GOTO(DRV_IOCTL(lchip, queue_id, cmd, &p_buffer_info->port_thrd), ret, error0);
        cmd = DRV_IOW(DsErmQueueLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_GOTO(DRV_IOCTL(lchip, queue_id, cmd, &buffer_info_tmp.port_thrd), ret, error0);

        /* queue guaranteed */
        cmd = DRV_IOR(DsErmQueueGuaranteedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_GOTO(DRV_IOCTL(lchip, chan_id, cmd, &p_buffer_info->port_guaranteed), ret, error0);
        cmd = DRV_IOW(DsErmQueueLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_GOTO(DRV_IOCTL(lchip, chan_id, cmd, &buffer_info_tmp.port_guaranteed), ret, error0);   
    }
    else
    {
        /* port limited */
        cmd = DRV_IOR(DsErmPortLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_GOTO(DRV_IOCTL(lchip, chan_id, cmd, &p_buffer_info->port_thrd), ret, error0);
        cmd = DRV_IOW(DsErmPortLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_GOTO(DRV_IOCTL(lchip, chan_id, cmd, &buffer_info_tmp.port_thrd), ret, error0);

        /* port guaranteed */
        cmd = DRV_IOR(DsErmPortGuaranteedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_GOTO(DRV_IOCTL(lchip, chan_id, cmd, &p_buffer_info->port_guaranteed), ret, error0);
        cmd = DRV_IOW(DsErmPortGuaranteedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_GOTO(DRV_IOCTL(lchip, chan_id, cmd, &buffer_info_tmp.port_guaranteed), ret, error0);
    }
    
    ctc_vector_add(p_usw_queue_master[lchip]->buffer_info_vec, chan_id, p_buffer_info);
    return CTC_E_NONE;

error0:
    mem_free(p_buffer_info);
    return ret;
}

STATIC int32
_sys_tmm_qos_restore_buffer_thrd(uint8 lchip, uint16 queue_id, uint32 chan_id, uint8 pfc_en)
{
    sys_qos_buffer_info_tmm_t* p_buffer_info = NULL;
    ErmMiscCtl_m erm_misc_ctl;
    uint32 cmd = 0;

    p_buffer_info = ctc_vector_get(p_usw_queue_master[lchip]->buffer_info_vec, chan_id);
    if (!p_buffer_info)
    {
        return CTC_E_NONE;
    }
    cmd = DRV_IOR(ErmMiscCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &erm_misc_ctl));
    SetErmMiscCtl(V, guaranteedMode_f, &erm_misc_ctl, 7);/*bit0:porttc 1:portsc 2:port guarantee*/
    SetErmMiscCtl(V, thrdPassMode_f, &erm_misc_ctl, 7);
    cmd = DRV_IOW(ErmMiscCtl_t, DRV_ENTRY_FLAG);
    CTC_ERROR_RETURN(DRV_IOCTL(lchip, 0, cmd, &erm_misc_ctl));

    if(pfc_en)
    {
        /* queue limited */
        cmd = DRV_IOW(DsErmQueueLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, queue_id, cmd, &p_buffer_info->port_thrd));

        /* queue guaranteed */
        cmd = DRV_IOW(DsErmQueueLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, queue_id, cmd, &p_buffer_info->port_guaranteed));   
    }
    else
    {
        /* port limited */
        cmd = DRV_IOW(DsErmPortLimitedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &p_buffer_info->port_thrd));

        /* port guaranteed */
        cmd = DRV_IOW(DsErmPortGuaranteedThrdProfile_t, DRV_ENTRY_FLAG);
        CTC_ERROR_RETURN(DRV_IOCTL(lchip, chan_id, cmd, &p_buffer_info->port_guaranteed));
    }

    ctc_vector_del(p_usw_queue_master[lchip]->buffer_info_vec, chan_id);
    mem_free(p_buffer_info);

    return CTC_E_NONE;
}

int32
sys_tmm_qos_fcdl_recover_drop_pkt(uint8 lchip, void* p_sys_queue_node_param,  uint16 queue_id, uint8  chan_id, uint8 enable, uint8 pfc_en)
{
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_FUNC, "%s()\n", __FUNCTION__);
    SYS_QOS_QUEUE_DBG_OUT(CTC_DEBUG_LEVEL_PARAM, "drop pkt: queue_id=%d, chan_id=%d, enable=%d, pfc_en=%d\n", queue_id, chan_id, enable, pfc_en);

    if(enable) /*store profile*/
    {
        CTC_ERROR_RETURN(_sys_tmm_qos_store_buffer_thrd(lchip, queue_id, chan_id, pfc_en));
    }
    else
    {
        CTC_ERROR_RETURN(_sys_tmm_qos_restore_buffer_thrd(lchip, queue_id, chan_id, pfc_en));
    }

    return CTC_E_NONE;

}

#endif


